Atty. Dkt. No.: 02AB200 (081696-0251) 



U.S. PATENT APPLICATION 



for 



METHOD AND SYSTEM FOR GENERATING 
MULTI-DIMENSIONAL MOTION PROFILES 



Inventor(s): Juergen K. Weinhofer 



Atty. Dkt. No.: 02AB200 (081696-0251) 



METHOD AND SYSTEM FOR GENERATING 
MULTI-DIMENSIONAL MOTION PROFILES 

BACKGROUND OF THE INVENTION 

1 . Field 

[0001] This invention relates to a method and system apparatus for 
generating multi-dimensional motion profiles. In a particularly preferred 
embodiment, the invention relates to a method and apparatus for generating 
multi-dimensional motion profiles in a motion control system, such as in an 
industrial control system. 

2. Description of Related Art 

[0002] Motion control systems are known for controlling motors. Motors may 
be electric, hydraulic, or any other type of physical motor. For example, motion 
control systems have been used in industrial control systems for manufacturing, 
processing, packaging, testing, automation, measurement, robotic, material 
handling, assembling, cutting (metal, laser and others), grinding, welding, pick 
and place and other applications. In such systems, motion control systems are 
used to control motion of a controlled element, such as a tool (drill bit or other 
machine tool), a robotic arm (e.g., a pick-and-place mechanism at a tip of the 
robotic arm), actuator, sensor, or other device. Herein, the term "controlled 
element" refers broadly to any device, apparatus, and structure of interest or 
portion thereof whose position is controlled. 

[0003] In some systems, the position of a controlled element is determined by 
the positions of multiple motors. In such systems, it would be desirable to be 
able to control the motors in a coordinated fashion. Particularly, it would be 
desirable to be able to generate multi-dimensional motion profiles for the 
controlled element, and to be able to control the motors in a coordinated fashion 
in accordance with the multi-dimensional motion profiles. Further, it would also 
be desirable to be able to perform coordinate transformations between Cartesian, 
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cylindrical, and spherical coordinate systenns as well as forward and inverse 
kinennatic transformations. An ongoing need exists for systems and methods 
which are able to perform these functions. 

[0004] It should be understood that the teachings herein may also be used to 
achieve other advantages in addition to or different than the above-mentioned 
advantages. 

SUMMARY OF THE INVENTION 

[0005] According to a first preferred embodiment, a system comprises control 
logic and a programming interface. The programming interface is configured to 
permit a user to specify a plurality of weighting points in a multi-dimensional 
coordinate space. The control logic includes spline computation logic configured 
to generate a spline curve based on boundary conditions. The spline curve 
extends near the weighting points. The control logic is configured to generate 
control signals to control operation of a plurality of motion axes to drive 
movement of a controlled element along a path defined by the spline curve. 

[0006] According to another preferred embodiment, a control method for 
controls movement of a controlled element in a multi-dimensional coordinate 
system. The method comprises receiving a plurality of weighting points by way 
of a user instruction, generating a plurality of control points for a plurality of 
adjacent spline segments based on boundary conditions and the plurality of 
weighting points, generating a plurality of coefficient vectors for the plurality of 
spline segments based on the plurality of control points, generating a first 
plurality of position reference values based on the plurality of coefficient vectors 
and using the first plurality of position reference values to control a first motion 
axis, and generating a second plurality of position reference values based on the 
plurality of coefficient vectors and using the second plurality of position 
reference values to control a second motion axis. The first motion axis operates 
in a first dimension of the multi-dimensional coordinate system and the second 
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motion axis operates in a second dimension of the multi-dimensional coordinate 
system. 

[0007] According to another preferred embodiment, a system comprises 
motion control logic configured to control a first motion axis and a second 
motion axis in accordance with a user program. The motion control logic 
provides a plurality of instructions configured for use in the user program. The 
plurality of instructions include an instruction that permits a move to be specified 
by specifying weighting points for a spline path to be followed by the controlled 
element in a multi-dimensional coordinate system that includes the first motion 
axis and the second motion axis. 

BRIEF DESCRIPTION OF THE DRAWINGS 

[0008] FIG. 1 A is an overview of a control system that incorporates a 
reference value generation system and method according to an exemplary 
embodiment; 

[0009] FIG. IB is a schematic view of a motion controller, servo drive and 
motor of FIG. 1 A shown in greater detail; 

[0010] FIG. 2 is a functional block diagram showing building blocks embodied 
in a programming interface usable to program the control system of FIGS. 1A-1B 
according to an exemplary embodiment; 

[0011] FIGS. 3-4 are examples of multi-dimensional path profiles that may be 
generated for use in connection with the control system of FIGS. 1A-1B 
according to an exemplary embodiment; 

[0012] FIG. 5 is a functional block diagram showing a coordinated move block 
of FIG. 2 in greater detail; 

[0013] FIG. 6 is a functional block diagram showing a coordinate transform 
block of FIG. 2 in greater detail; 
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[0014] FIG. 7 is a functional block diagrann showing the coordinate 
transformation block of FIG. 6 in combination with the coordinated move block 
of FIG. 2; 

[0015] FIGS. 8-14 are examples of coordinate systems that may be used in 
programming the control system of FIGS. 1A-1B according to an exemplary 
embodiment; 

[0016] FIG. 1 5 is an example of a linear move that may be programmed for 
use in connection with the control system of FIGS. 1A-1B according to an 
exemplary embodiment; 

[0017] FIG. 16 is an example of a circular move that may be programmed for 
use in connection with the control system of FIGS. 1A-1B according to an 
exemplary embodiment; 

[0018] FIG. 17 is an example of a blended move that may be programmed for 
use in connection with the control system of FIGS. 1A-1B according to an 
exemplary embodiment; 

[0019] FIG. 18 is an instruction faceplate for a coordinate move instruction 
according to an exemplary embodiment; 

[0020] FIG. 19 is an instruction faceplate for a coordinate transformation 
instruction according to an exemplary embodiment; 

[0021] FIG. 20 is an instruction faceplate for a calculate target position 
instruction according to an exemplary embodiment; 

[0022] FIG. 21 is an instruction faceplate for a change coordinated dynamics 
instruction according to an exemplary embodiment; 

[0023] FIG. 22 is an instruction faceplate for a stop instruction according to 
an exemplary embodiment; 
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[0024] FIG. 23 is an instruction faceplate for a linear coordinated move 
instruction according to an exemplary embodiment; 

[0025] FIG. 24 is an instruction faceplate for a circular coordinated move 
instruction according to an exemplary embodiment; 

[0026] FIG. 25 shows an example of a B6zier spline curve which may be 
generated according to another exemplary embodiment; 

[0027] FIG. 26 shows a process for generating position reference values as a 
function of time for the embodiment of FIG. 25; 

[0028] FIG. 27 shows a transition from a preceding motion profile to a B6zier 
spline segment for the embodiment of FIG. 25; 

[0029] FIG. 28 shows another transition from a preceding motion profile to a 
Bezier spline segment for the embodiment of FIG. 25; 

[0030] FIG. 29 shows a transition from a preceding circular motion profile to a 
Bezier spline segment for the embodiment of FIG. 25; 

[0031] FIG. 30 shows a two-segment B6zier spline that blends from one linear 
motion profile to another linear motion profile for the embodiment of FIG. 25; 
and 

[0032] FIG. 31 shows a two-segment Bezier spline that blends from a linear 
motion profile to a circular motion profile for the embodiment of FIG. 25. 

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 

[0033] Various preferred embodiments of the invention will now be described. 
Although the invention is described by way of example in the context of an 
industrial control system, it will be appreciated that the invention may be 
employed in other types of systems as well. 
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I. PREFERRED SYSTEM ARCHITECTURE 

A. Hardware Configuration 

[0034] Referring now to FIGS. 1A-1B, FIG. 1A shows an overview of a control 
system 10 in which a multi-dimensional motion profile is generated in 
accordance with a preferred embodiment. Although the control system 10 is 
shown to be an industrial control system, it will be appreciated that the methods 
and systems described herein may also be applied in other contexts. 

[0035] The control system 10 comprises a plurality of industrial controllers 12 
that are interconnected by way of a communication network 14. The control 
system 10 also includes a plurality of input devices 16 and a plurality of output 
devices 17. The output devices 17 are controlled by the controllers 12 in 
response to input status information from the input devices 16. In one 
embodiment, the industrial controllers 12 are programmable logic controller (PLC) 
systems. Other hardware configurations may also be used. 

[0036] The control system 10 operates based on execution of a user program 
that is tailored to the industrial control system and the particular manufacturing, 
processing, packaging, automation, testing, measurement, robotic and/or other 
application(s) for which it is used. In the illustrated embodiment, the control 
system 10 includes multiple industrial controllers 12, and the user program may 
comprise one or more subprograms that are executed by the industrial 
controllers 12. In a simpler industrial control system, such as in the case of a 
stand-alone motion control system, the user program may be contained in a 
single motion controller. 

[0037] At least some of the controllers 12' include an output device 17 which 
is a motion control system 20. The controllers 12' include a motion controller 
23 which is connected by way of a servo drive 25 to a motor 27. A feedback 
sensor 29, which may be integrated with the motor 27 or provided separately, is 
also provided for position/velocity feedback. The feedback sensor 29 may, for 
example, be an encoder or a resolver. The motion controller 23 may be provided 
as a separate physical module, as program logic included in the controller 12', or 
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in a variety of other configurations. Likewise, as previously noted, the motion 
controller 23 may also be integrated with the servo drive 25 and/or integrated 
with the motor 27. Other configurations are also possible. 

[0038] The interconnection of the motion controller 23, the servo drive 25, 
the motor 27 and sensor 29 for an exemplary hardware configuration is shown 
in greater detail in FIG. I B. As shown therein, the system 10 also includes a 
user program 30, which may be executed by the industrial controller 1 2' and/or 
by the motion controller 23. The user program 30 includes one or more multi- 
axis coordination instructions 31, described in greater below, as well as other 
instructions 32. 

[0039] In combination, the motion controller 23 and the servo drive 25 
implement a position control loop 34, a velocity control loop 38, and a torque or 
current control loop 42. The position control loop 34, which is shown to include 
a PI (proportional-integral) gain element 36, operates to minimize the difference 
between a commanded position of the rotor shaft (provided by the user program) 
and the actual position of the rotor shaft (provided by the sensor 29). The 
velocity control loop 38, which is nested within the position control loop and is 
shown to include a PI gain element 40, operates to minimize the difference 
between a commanded rotor shaft velocity (provided by the position control loop 
34) and the actual velocity of the rotor shaft (determined based on feedback 
information provided by the sensor 29). The torque/current control loop 42 is 
nested within the velocity control loop 38. (Because torque/current magnitudes 
are proportional, the same control loop can be viewed as either a torque control 
loop or a current control loop.) The torque/current control loop 42, which is 
shown to include a PI gain element 44, operates to minimize the difference 
between a commanded torque/current (received from the velocity control loop) 
and the actual torque/current (measured using current sensors, not shown). In 
combination, therefore, the control loops 34, 36 and 38 are capable of accepting 
a position command and in response generating current to drive the motor and 
thereby to control the motor to obtain the commanded position. The location of 
the position, velocity and current loops is not important. Additionally, other loop 
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configurations are also possible, which may contain any combination of feed 
forward loops, open loops, closed loops, or nested loops etc. 

[0040] In operation, to move from one position to another, a series of 
intermediate position reference values are generated for or by the motion 
controller 23. Thus, if the motor is to move to a new position Xend, then a series 
of incremental position reference values Xrefci) . . . Xrefim are generated that 
extend from an initial position Xo to the new position Xend, The incremental 
position reference values are generated by logic commonly referred to as an 
"interpolator." An example of an interpolator is disclosed in U.S. Pat. No. 
6,300,738, entitled "Method and Apparatus for Generating Reference Values for 
a Motion Control Loop in an Industrial Control System," hereby incorporated by 
reference. In practice, the position reference values may be provided in the form 
of delta position information (change in position since the last update) rather 
than absolute position information. The position reference values may be 
updated each millisecond, for example. 

[0041] The incremental position reference values are provided as position 
commands to the position feedback control loop 34 with a new position 
reference value being provided every few milliseconds or so. In this way, as 
new position references are generated, the motor "chases after" or follows the 
position references until the position Xend is reached. 

[0042] Typically, the interpolator generates the incremental position reference 
values responsive to encountering an instruction in a user program in which the 
motor 27 is instructed to move from a current position to a new position, with 
the new position being specified as a parameter of the instruction. Such an 
instruction may be a single dimensional instruction, in which the move 
instruction commands only a single motor to move to a new position. Herein, 
such an instruction is referred to simply as a "move" instruction. 

[0043] In the system 10, multi-axis coordination instructions 31 are preferably 
also provided. For example, as described in greater detail below, a coordinated 
move instruction 200 (FIG. 18) is preferably also provided that allows an 
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operator to program multi-dimensional moves and allows the motion of multiple 
motors to be coordinated responsive to the user-programmed multi-dimensional 
move/path profile. This permits a single instruction to be used to program 
movement of a controlled element to move from a current position to a new 
position, wherein the movement to the new position involves the action of more 
than one motor 27. Likewise, a coordinate transformation instruction 300 
(FIG. 19) allows a user to specify a move in one coordinate system and have the 
move implemented in another coordinate system. The multi-axis coordination 
instructions 31 are described in greater detail below. 

B. Programming Interface 

[0044] A variety of different programming interfaces may be used to control 
the control system 10 of FIGS. 1A-1B. Such programming interfaces may be 
based on standard sequential programming languages programming languages 
(e.g., C, C + +, Visual Basic, etc), programming languages used for industrial 
controls (e.g. ladder diagram, structured text, function block, sequential function 
chart, flow charts, GML etc) or motion control programming (e.g., computer 
numeric control or CNC-type languages, such as RS274D), other sequential 
programming languages, and so on. 

[0045] In the preferred embodiment, an arrangement such as that shown in 
FIG. 2 is employed. Referring now to FIG. 2, FIG. 2 is a functional block 
diagram showing building blocks embodied in a programming interface usable to 
program the control system 10. The building blocks include one or more motion 
control axis blocks 52, one or more output cam blocks 54, one or more output 
blocks 56, and a plurality of motion control blocks 58. The motion control 
blocks 58 further include one or more jog blocks 62, one or more move blocks 
64, one or more time cam blocks 66, one or more gear blocks 68, one or more 
position cam blocks 70, one or more coordinated move blocks 72, and one or 
more coordinate transformation blocks 74. The motion control blocks 58 also 
include an adder block 75, which combines inputs (e.g., incremental position 
command values) from the remaining motion control blocks 58. 
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[0046] In one embodiment, the building blocl<s 52-58 are implemented as 
objects in an object-oriented programming environment. In an object-oriented 
programming environment, individual objects typically represent different 
physical objects (e.g., motors, sensors, etc.), relationships between physical 
objects (e.g., camming relationships, gearing relationships, etc), and so on. The 
objects are then accessed by instructions in a user program to affect operation 
of the physical objects or relationships therebetween. Thus, the axis blocks 52 
may each be implemented as an object having services that may be invoked 
using one or more axis instructions, and the output cam blocks 54 may be 
implemented as an object having services that may be invoked using one or 
more output cam instructions. Further, each of the motion control blocks 58 
may be implemented as objects (e.g., a jog object, a move object, a time cam 
object, a gear object, a position cam object, a coordinated move object, and a 
coordinate transformation object) having services that may be invoked using one 
or more respective instructions (e.g., a jog instruction, a move instruction, a time 
cam instruction, a gear instruction, a position cam instruction, a coordinated 
move instruction, and a coordinate transformation instruction). 

[0047] In designing such a system, new instances of the above-mentioned 
objects may be created to represent new/additional devices or relationships 
between devices. Thus, although only a small number of building blocks 52-58 
is shown, it will be appreciated that this configuration is purely for purposes of 
explanation and that practical systems are likely to have a configuration different 
than that shown in FIG. 2, typically with many additional ones of the building 
blocks 52-58. For example, new instances of the axis object may be created to 
represent each additional motion control axis (e.g., a motor, a shaft or other 
device driven by a motor, virtual axis, and so on), and various axis instructions 
may be incorporated into a user program to invoke the services of the axis 
objects. The axis instructions may then be executed to control movement of the 
motion control axes (e.g., to control a position such as an absolute position, 
relative position, delta position, etc., of the motion control axis). Other 
instructions may be executed to control output devices using the services of the 
output objects 56, for example, as a function of motor position (actual or 
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measured) using output cam objects 54. Typically, the operation of the output 
devices 50 will at least to some extent be cyclic, and the output cam objects 54 
may be used to synchronize the operation of the output devices to rotation of 
the shaft of a motor 27 (or to movement of a device driven by the shaft of the 
motor 27). 

[0048] The motion control blocks 58 allow the motion control axis to be 
controlled in different ways. The jog block 62 permits the user (via a jog 
instruction) to specify a new velocity at which the shaft of the motor 27 is to 
move. The move block 64 permits the user (via a move instruction) to specify a 
new position for the shaft of the motor 27. The time cam block 66 permits the 
user (via a time cam instruction) to specify an axis position profile which 
specifies axis position as a function of time. The gear cam block 68 permits the 
user (via a gear instruction) to specify an electronic gearing relationship between 
the shaft of the motor 27 and the shaft of another motor (not shown) in the 
system 10. The position cam block 70 permits the user (via a position cam 
instruction) to specify an axis position profile which specifies axis position for 
the shaft of the motor 27 as a function of a position of the shaft of another 
motor (not shown) in the system 10. In the case of the gear block 64 and the 
position cam block 70, it will be appreciated that these blocks will receive inputs 
from other axis blocks similar to the axis block 52, which may in turn receive 
inputs from motion blocks similar to the motion blocks 58. The coordinate move 
block 72 and the coordinate transformation block 74 are described in greater 
detail below. It is therefore seen that the complexity of the motion control 
system 1 0 can be increased by providing multiple axes (and therefore multiple 
motors or other devices) and defining various relationships between the axes and 
other devices. Fewer, more, or different building blocks and instructions may 
also be used. Of course, it should be understood that these features, and all of 
the other features described herein, may also be implemented without using 
object-oriented techniques. 

[0049] Referring back to FIGS. 1A-1B, each of the motors 27 is represented 
by an axis block 52 that is stored as control logic in the industrial controllers 12' 
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(or motion controllers 23). It nnay also be noted, however, that virtual axes may 
also be used. Accordingly, in some cases, axis blocks 52 may be used in 
situations where there is no associated physical motor. 

[0050] The control logic associated with each of the axis blocks 52 generates 
control signals for a respective one of the motors 27 responsive to inputs from 
the adder block 75. For example, in the preferred embodiment, described in 
greater detail below, the coordinated move block generates incremental position 
reference commands for each motion control axis block 52. Each axis block 52 
uses the position reference commands as input to the nested feedback control 
arrangement described above in connection with FIG. IB. The position of the 
motor 27 is monitored by the feedback device 29 which provides feedback 
information to the axis block 52. Alternatively, the feedback device 29 may be 
used to monitor the position of a device driven by the motor 27. 

II. COORDINATED MOVE AND COORDINATE TRANSFORMATION BLOCKS 

[0051] The coordinated move block 72 and the coordinate transformation 
block 74 will now be described in greater detail. The coordinated move block 72 
provides a programmer of the system 10 with user-friendly mechanisms for 
controlling motion of multiple motion control axes (e.g., multiple motors 27) in a 
coordinated fashion. The coordinate transformation block 74 provides the 
programmer with a mechanism for relating the axes of one coordinate system to 
the axes of another coordinate system. Although two axes are shown, it will be 
understood that the blocks 72 and 74 may be used in connection with three 
axes or any other number of axes. If an object oriented approach is used, 
interfaces to the coordinated move block 72 and the coordinate transformation 
block 74 may be provided by way of one or more multi-axis coordination 
instructions 31, described in greater detail in Section III, below. 

[0052] The coordinated move block 72 preferably provides an interface for 
linear moves in multi-dimensional Cartesian space and circular moves in two and 
three-dimensional Cartesian space. The preferred algorithm used for the path 
planning permits dynamic parameter changes for maximum velocity. 
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acceleration, and deceleration along the path. It also provides smooth transitions 
(without spikes in acceleration) from one coordinated move path segment to the 
next. 

[0053] The coordinate transformation block 74 preferably provides an 
interface for relating the axes of a source coordinate system to the axes of the 
target coordinate system. The possible source and target systems may include 
Cartesian coordinate systems in multi-dimensional space, cylindrical and 
spherical coordinate systems in two- and three-dimensional space, and 
independent and dependent kinematic coordinate systems in two- and three- 
dimensional space. The coordinate transformation block 74 enables translation 
and rotation from the source to the target system. 

[0054] Referring now to FIGS. 3-4, examples of the operation of the 
coordinated move block 72 are shown. FIGURES 3-4 both show path profiles 
followed by a controlled element that is controlled by the coordinated move 
block 72. In FIG. 3, an exemplary two-dimensional path profile is shown which 
may be executed, for example, in the context of a pick-and-place system. Thus, 
as shown in FIG. 3, a pick-up mechanism may pick up a part at point A, move 
up along path segment 82, move over along path segment 84, and move down 
along path segment 86 to a placement point B. The path segments 82, 84 and 
86 are not tangential to each other. Accordingly, transitioning path segments 
88 and 90 are also shown which extend between and connect the path 
segments 82, 84 and 86. The transitioning path segments 88 and 90 transition 
movement from trajectories along the path segments 82 and 84 to trajectories 
along the path segment 84 and 86, respectively. In the context of the pick-and- 
place example, the transitioning path segments 88 and 90 allow the pick-and- 
place mechanism to smoothly transition between the path segments 82, 84 and 
86 while maintaining a smooth acceleration profile and while not having to slow 
down. The path segments 88 and 90 may have shapes which are optimized for 
smooth path transitions and need not be circular segments. 
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[0055] Preferably, for programming convenience, the programming interface 
permits the programmer to specify only the path segments 82, 84 and 86 and 
the programmer is not required to also specify the transitioning path segments 
88 and 90. It may be the case that the programmer is not particularly 
concerned about the exact manner in which the transitioning path segments are 
executed, and therefore this arrangement permits the programmer to program 
the overall path profile by programming only three path segments instead of five, 
which is more convenient. 

[0056] The path segments 82, 84 and 86 may be specified as parameters in 
user instructions of a user program. For example, a separate coordinated move 
instruction may be used to program each of the segments 82, 84 and 86. In 
FIG. 3, the path segments 82, 84 and 86 are shown to be linear path segments. 
For each linear path segment, the coordinated move instruction may be used to 
specify endpoints 92 of the path segment and/or transition points or other 
parameters indicating when the move may begin transitioning from one linear 
path segment to the next. The additional transitioning path segments 88 and 90 
are generated automatically by the coordinated move block 72 and do not need 
to be specified by the user. Of course, although the path profile shown in FIG. 3 
comprises a series of user-specified non-tangential path segments 82, 84 and 86 
connected by transitioning path segments 88 and 90, the user is also permitted 
specify path segments that are tangential (such that there is no need for 
transitioning path segments) for other portions of the overall path. 

[0057] In operation, movement of the controlled element is first driven along 
the segment 82 responsive to a first coordinated move instruction in a user 
program. When the transition point 92 is reached, the coordinated move block 
72 operates to control movement of the controlled element along the transition 
path segment 88. This process continues until the placement point B at the end 
of path segment 86 is reached. Typically, the transition from one path segment 
to the next uses the end dynamics of the current path segment as the start 
dynamics for the next path segment. 
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[0058] In FIG. 4, two path segments 94 and 96 specified by a user and an 
additional transitioning path segment 98 are shown. As shown in FIG. 4, the 
path segment 94 is generally linear and the path segment 96 is generally 
circular. In one embodiment (FIG. 18), the same coordinated move instruction 
may be used by an operator to program both types of moves. In another 
embodiment (FIGS. 23-24), one instruction may be used to program linear 
moves and another instruction may be used to program circular moves. 

[0059] As shown in FIG. 4, the path segment 94 and the path segment 96 are 
located in different three-dimensional planes. The path segment 94 is generally 
normal to the plane defined by the path segment 96 and, accordingly, there is no 
single three dimensional plane that contains both path segments 94 and 96. 
Nevertheless, the coordinated move block 72 is able to generate the 
transitioning path segment 98 which is able to transition between the non- 
tangential, non-coplanar path segments. The. transitioning path segment need 
not be specified by a user, which makes it easier for the user to program the 
system 10. 

[0060] As will be seen below, in the coordinated move instruction 200, the 
user specifies a group of coordinated motion control axes (e.g., corresponding to 
the motors 27) which define the target coordinate system for the move. 
Generally speaking, the target coordinate systems may be different from one 
path segment to the next. With reference to FIG. 5, each coordinated move is 
preferably transformed into multiple individual orthogonal moves (represented as 
move blocks 76) of a source coordinate system during initial execution of a 
coordinated move instruction 200. In the context of the coordinated move block 
72, the source coordinate system is defined by the coordinated move block 72 
and is not user-specified. As detailed below, in one embodiment, the source 
coordinate system is defined based on the trajectory of the path segment. For 
example, the source coordinate system may be defined as having one axis which 
is tangential to the path segment, along with additional axes which are normal 
and bi-normal to the tangential axis. 
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[0061] In one embodiment, for the transitioning path segments, the source 
coordinate system is defined relative to the next path segment (e.g., when 
transitioning from path segment 82 to path segment 84, a source coordinate 
system defined based on the path segment 84 may be used). As noted 
previously, however, the transition from one path segment to the next preferably 
uses the end dynamics of the current path segment as the start dynamics for the 
next path segment. Accordingly, in a more preferred embodiment, as the move 
transitions dynamically from the source coordinate system that was used in 
connection with path segment 82 to the source coordinate system to be used in 
connection with path segment 84, the move's dynamics (position, velocity and 
acceleration) change gradually from one path segment to the next path segment. 
This arrangement facilitates providing a smooth acceleration profile as the 
transition from one path segment to the next path segment occurs. The 
interpolators are reinitialized with each path segment and the orientation of the 
interpolators remains constant during the execution of each path segment. 

[0062] Because each coordinated move is transformed into multiple individual 
orthogonal moves, a separate interpolator may be provided for each orthogonal 
move (that is, each move block 76 may be provided with its own separate 
interpolator). Each interpolator provides a separate respective set of incremental 
position reference values for each of the axes of the source coordinate system 
during a dynamic phase of the move instruction. Assuming for example a three 
dimensional source coordinate system, three interpolators may be used which 
each generate a set of incremental position reference values in the source 
coordinate system. The incremental position reference values are generated 
substantially simultaneously by the interpolators, e.g., in accordance with regular 
update cycles (which may for example be in the range of hundreds of 
milliseconds to microseconds). 

[0063] The coordinated move block 72 includes coordinate transformation 
logic which performs a coordinate transformation to transform the incremental 
position reference values commands from the source coordinate system to a 
target coordinate system. Whereas the source coordinate system is preferably 
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defined based on the trajectory of the move, the target coordinate system is 
preferably defined based on the motion control axes which correspond to the 
axes of movement of three respective motors, as previously noted. The 
coordinate transformation logic is therefore able to transform the incremental 
position reference values generated by the move blocks 76 in the source 
coordinate system to position reference values which are useable by the motion 
controllers 23 in the target coordinate system. 

[0064] With reference to FIG. 2, the move blocks 76 are connected to the 
coordinated move block 72. As a result, the outputs of the individual move 
objects 76 provide the inputs (incremental position reference values) to the 
coordinated move block 72, and the coordinated move block 72 is responsible 
for transforming the individual moves back into the target coordinate system and 
supplying these coordinates to the axis blocks 52. After completion of the 
individual moves, the target coordinates are preferably adjusted to the target 
position values, to accommodate any rounding errors in the transformation. This 
routine may be executed in the coordinated move block 72. 

[0065] With reference to FIG. 6, the coordinate transformation block 74 
preferably provides an interface for relating the axes of a source coordinate 
system to the axes of the target coordinate system. As shown in FIG. 6, the 
axes of a source system are connected to the coordinate transformation block 
74. The coordinate transformation block 74 is used to transform the 
incremental position reference values in the source coordinate system from the 
axis blocks 78 into incremental position reference values in the target coordinate 
system for the axis blocks 80. 

[0066] With reference to FIG. 7, the coordinated move block 72 and 
coordinate transformation block 74 may also be used in combination. For 
example, in the case of a two-axis robot, it may be desirable to control the 
shoulder and elbow axis of the robot by commanding a path in Cartesian space. 
In this configuration, the coordinated move block 72 may provide the command 
position for the x- and y-axis. The coordinate transformation block 74 may then 
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be responsible for transforming the x- and y-axis positions into the shoulder and 
elbow axis positions. 

[0067] The source axis may provide either the actual or command coordinates 
as input for the coordinate transformation block 74. This allows multi- 
dimensional interpolation to operate in one domain (e.g., an x-y coordinate 
system) even though the hardware (e.g., motors, feedback devices) are 
operating in another domain (e.g., a shoulder-axis, elbow-axis coordinate 
system). 

[0068] In FIGS. 5-7, the coordinated move block 72 and the coordinate 
transformation block 74 are shown as being connected to multiple axis blocks 
78. Each axis block 78 is associated with a different corresponding motor. 
However, this relationship may be direct or indirect. For example, each of the 
axis blocks 78 may directly correspond to an associated motor. Alternatively, 
one or more of the axis blocks 78 may be a virtual axis which is connected by 
way of one or more other blocks (not shown) to an associated motor. 

[0069] As apparent from the above discussion, in the preferred embodiment, 
the coordinated move path is calculated using multi-dimensional interpolation 
with coordinate transformation. This approach may offer such advantages as 
support for dynamic path transitions, smooth path transitions, independent 
specification of acceleration and deceleration, and relatively easy 
implementation. Other approaches, such as one-dimensional interpolation with 
coordinate transformation, superimposed interpolation of two profiles with 
coordinate transformation, interpolation with finite impulse response filters, 
interpolation with non-uniform rational B-splines, and interpolation with Bezier 
splines may be used. In some cases, these other approaches offer faster speed 
for short move distances. As will be appreciated, each of these other 
approaches offers their own advantages and disadvantages. 

[0070] The coordinated move block 72 and the coordinate transformation 
block 74 preferably support a variety of coordinate systems. Section 11(A) 
describes examples of coordinate systems that may be supported and associated 
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coordinate transformation equations. Section 11(B) describes examples of 
supported coordinated moves. Section 11(C) describes examples of auxiliary 
algorithms that may be used to derive the transformation matrices. Section 11(D) 
describes examples of instruction interfaces for coordinated moves and the 
relationship to the coordinated move block 72 and the coordinate transformation 
block 74. The equations and algorithms in Sections ll(A)-ll(C) may be embodied 
in and employed by the coordinated move block 72 and the coordinate 
transformation block 74 in connection with providing services invoked by the 
instructions described in Section 11(D). While Sections ll(A)-ll(D) provide 
examples of mathematical equations, algorithms, instruction interfaces, and 
techniques that may be used to implement the functionality described herein, it 
will be appreciated that other equations, algorithms, interfaces and techniques 
may also be used. 

A. Coordinate System Description 

[0071] The coordinated move block 72 and the coordinate transformation 
block 74 preferably supports a variety of different coordinate systems. These 
coordinate systems may include, for example, multi-dimensional Cartesian 
coordinate systems, two- and three-dimensional cylindrical, and spherical 
coordinate systems as well as two and three-dimensional independent and 
dependent coordinate systems. The two-dimensional cylindrical and spherical 
coordinate system can also be viewed as polar coordinate system. The 
independent and dependent coordinate systems represent specific kinematic 
relationships. Of course, depending on the application, additional and/or 
different coordinate systems may also be used. 

[0072] For purposes of mathematical characterization, each coordinate system 
is specified by a coordinate origin, a coordinate basis, and a scale of 
measurement along the basis axes or curves. With these specifications, a point 
in space is defined by a set of coordinates that specify the multitude of the scale 
of measurement along each basis axes or curve. 
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[0073] In addition to defining a point in space with one coordinate system, it 
also is desirable to be able to transform the coordinates of a point from one 
coordinate system to another. Coordinate transformation is also desirable for the 
implementation of the coordinated moves. The principal coordinate 
transformations are translation (shifting) and rotation of a Cartesian coordinate 
system and transforming cylindrical, spherical, independent, and dependent 
coordinate systems to and from a Cartesian coordinate system. All other 
transformations can be derived from these principal coordinate transformations. 
The coordinate transformations described in this section are used in both the 
coordinated move block 72 and the coordinate transformation block 74. 

[0074] This section describes these various coordinate systems. The 
following table provides a description of vector math notations used herein: 



NOTATION 


DESCRIPTION 


X 


Scalar value 


P = 


(X1, 


Xn) 


Point or Position (text format) 




x^ 




Point or Position (equation format) 


P = 
















v = [v,,. 




Vector (text format) 


V = 


V, 




Vector (equation format) 


-T 
V = 




- vj 


Transposed vector (equation format) 


M = 




....vj 


Matrix (text format) 
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M = 


[v, ... vj 
V ... V 

_ Im nm _ 




Matrix (equation format) 


T 

M = 


Vi 

-r 
v„ 


ntn _ 




Transposed matrix (equation format) 




Basis of a coordinate system 


atan (xi, x^) 


Angle between the vector from the 
origin 0 to the point P (xi, xi) and the 

positive -axis with the angle limited to 

the range (-71, k). 


-r — 

V w = v,>Vj + — l-v^w^ 


Inner product of two vectors 


vxw = 


V3W, - VjW3 
VjWj — V2WJ 




Outer product of two vectors 


V = V V V 


Length or norm of vector v 


detM 


Determinant of matrix M 


//,=(-iy^^detM, 


Cofactor corresponding to element rrnj of 
matrix M , The determinant det M m is 
called the //th minor of the matrix M , 
where My is the matrix obtained by 
deleting the /th column and /th row of 
the matrix M . 



Table I: Exemplary Mathematical Notations 



[0075] Herein, the term "axis" refers to a reference line of a coordinate 
system. The term "motion axis" refers to a linear or rotary axis having a linear 
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or angular coordinate. A motion axis may be physical hardware (e.g., a motor) 
or virtual (e.g., used for data processing purposes). The term "unit vector refers 
to a vector whose norm is one. The term "orthogonal vectors" refers to vectors 
whose inner product is zero. The term "orthonormal vectors" refers to 
orthogonal unit vectors. The term "orthogonal matrix" refers to a matrix whose 
column vectors are mutually orthonormal. 

1 . Cartesian Coordinate System 

[0076] Referring now to FIG. 8, FIG. 8 shows the geometric relationship of a 
three-dimensional Cartesian coordinate system 100. As shown in FIG. 8, the 
Cartesian coordinate system 100 is defined by a coordinate origin O and 
mutually orthogonal coordinate axes {x^, xj. With the same linear scale of 
measurement on the individual axes, the Cartesian coordinates (xi, Xn) define 
a point P in Cartesian coordinate space. 

[0077] Referring now to FIG. 9, a translation transformation may be used to 
shift the origin of a Cartesian coordinate system. FIG. 9 illustrates the geometric 
relationship between the source and target coordinate systems of the translation 
transformation. The coordinates (xi,t, xn,t) of a point Pt specified in the 
target system depend on the coordinates (xi,s, Xn,s) of the same point Ps 
specified in the source system and the offset vector o = (xi,o, Xn,o). 

[0078] The algebraic relationship between the coordinates {xi,s, Xn,s) of a 
point Ps specified in the source system and the coordinates (xi,t, Xn,t) of the 
same point Pt specified in the target system is defined by the following equation: 

Pt = Ps+o 









+ 








^n,5 _ 







The inverse transformation is defined by the following equation: 

-22- 

001.1499031.1 



Atty. Dkt. No.: 02AB200 (081696-0251) 
Ps = P,-o 



[0079] Referring now to FIG. 10, a rotation transformation may be used to 
rotate the axes of a Cartesian coordinate system. FIG. 10 illustrates the 
geometric relationship between the source and target coordinate systems in a 
rotation transformation. The coordinates (xi,t, Xn,t) of a point Pt specified in 
the target system depend on the coordinates (xi,s, xn,s) of the same point Ps 
specified in the source system and the rotation matrix R =[^,..., The 
columns of the rotation matrix are the basis vectors {r^, r„} of the source 
system specified in the target system. Since these vectors do not necessarily 
represent a mutually orthonormal basis, they are transformed into such a basis 
{^1, t„} by applying the auxiliary algorithm of Section 11(C). 

[0080] The algebraic relationship between the coordinates {x;,s, Xn,s) of a 
point Ps specified in the source system and the coordinates (X7,f, Xn,t) of the 
same point Pt specified in the target system is defined by the following equation. 




Since the transformation matrix T is orthogonal, the above equation can be 
restated. 
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= TPs 



— T 

T Pt 



-T 
ti 



-T 
tn 



-T- 



-T- 
tn t\ 



= IP, 



= P. 



-T- 
tx tn 



-T- 
t nt n 



Therefore, the inverse transformation is defined by the following equation. 

= f^P' 







~-T~ 








tx 








-T 








t« 


_•"■«,< _ 



Similarly, the algebraic relationship between a vector at the point Ps specified 
in the source systenn and the same vector Vt specified in the target system is 



defined by Vt =Tv^ and Vs =T Vt. 

2. Cylindrical Coordinate System 

[0081] Referring now to FIG. 1 1, FIG. 1 1 shows the geometric relationship of 
a three-dimensional cylindrical coordinate system 1 10. The coordinate system 
1 10 is defined by a coordinate origin O and three mutually orthogonal coordinate 
curves {r, tp, z} (tangents are orthogonal except along the X3-axis). With the 
same linear scale of measurement on the curves r and z and a radian scale of 
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measurement on the curve (p , the cylindrical coordinates (/-, q>, z) define a point 
P in cylindrical coordinate space. 

[0082] A transformation may be performed to transform coordinates in the 
cylindrical coordinate system to coordinates in a Cartesian coordinate system. 
Specifically, the algebraic relationship between the coordinates (r, ^, z) of a 
point Ps specified in the cylindrical system and the coordinates (xi, X2, xs) of the 
same point Pt specified in the Cartesian system is defined by the following 
equation. 

Pt = fcyl.-»Cart.(^) 



rcoscp 
rsing) 
z 



Similarly, the algebraic relationship between the coordinates (w, v^, vz) of a 
vector Vs specified in the cylindrical system and the coordinates {v^, V2, vs) of 
the same vector Vt specified in the Cartesian system at the point Ps is defined 
by the following equation. 

V, = g Cyl .-^Cart . (^j ' ) 

cos^-v^sin^ 
sin^ + v^cos^ 



[0083] A transformation may also be performed to transform coordinates in 
the Cartesian system to coordinates in cylindrical coordinate system. In general, 
the coordinates (x/, X2, xa) of a point Ps specified in the Cartesian system 
uniquely define the coordinates (/*, (p, z) of the same point Pt specified in the 
cylindrical system. In the special case that the point Ps is located on the X3-axis, 
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the coordinates (qt, q2, qa) of a vector q^, which is non-parallel to the :c3-axis, 

are used to uniquely define the coordinate cp. The algebraic relationship between 
the coordinates (xi, X2, xa) of the point Ps and the coordinates (r, (p, z) of the 
same point Pt is defined by the following equation. 



= f . 



Cart.^Cyl. 



[p.'i.) 



atan(A:2,Xj) 



0 



atan(^2.^i) 



for 7t 0 or ^2 0 



for = 0 and JC2 = 0 



Similarly, the algebraic relationship between the coordinates (v^, vz, v^) of a 
vector Vs specified in the Cartesian system and the coordinates (w, v^, vz) of the 

same vector Vt specified in the cylindrical system at the point Ps is defined by 
the following equation. 



~ 8cart.->Cyl. \ 

Vj cos^ + V2 sin^ 
-VjSin^ + V2Cos^ 
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V,X, + VjXj 



i 



+ xl 



V 



4 



0 



for jc, 5^ 0 or jCj 0 



for = 0, ^2 = 0, = Avj, 
and ^2 ^'^i 



for X, = 0, ^2 = 0, = /Iv, , 
and ^2 = ^^-^2 



3. Spherical Coordinate System 

[0084] Referring now to FIG. 12, FIG. 12 shows the geometric relationship of 
a three-dimensional spherical coordinate system 1 20. The coordinate system 
120 is defined by coordinate origin O and the three mutually orthogonal 

coordinate curves ^,^,^^} (tangents are orthogonal except along the j^cs-axis). 
With a linear scale of measurement on the curve r and a radian scale of 
measurement on the curves ^ and i// , the spherical coordinates [r, <p, define 
a point P in spherical coordinate space. 

[0085] A transformation may be used to transform coordinates in the spherical 
coordinate system to coordinates in the Cartesian coordinate system. 
Specifically, the algebraic relationship between the coordinates {r, <p, ifA of a 
point Ps specified in the spherical system and the coordinates {x^, xi, xz) of the 



-27- 



Atty. Dkt. No.: 02AB200 (081696-0251) 



same point Pt specified in the Cartesian system is defined by the following 
equation: 



= f< 



Sph.->Cart, 







rcos^cos^/ 


^2 




rsin^cos^ 


3. 




rsin^ 



Similarly, the algebraic relationship between the coordinates (w, v^, Vy) of a 

vector specified in the spherical system and the coordinates (v^i, V2, v^) of the 

same vector Vt specified in the Cartesian system at the point Ps is defined by 
the following equation. 



Iph.-^Cart 



cos^cos^-v^ sin^^?-v^ cos^sin^ 
sin (p cos y/-^v^ cos (p-v^ sin cp sin y/ 
sin yz-^v^ cos y/ 

[0086] A transformation may be also used to transform coordinates in the 
Cartesian coordinate system to coordinates in the spherical coordinate system. 
In general, the coordinates (xi, X2, xs) of a point Ps specified in the Cartesian 
system uniquely define the coordinates (r, % ^^/) of the same point Pt specified in 
the spherical system. In the special case that the point Ps is located on the JC3- • 
axis, the coordinates (qr;, q2, qa) of a vector q^, which is non-parallel to the JC3- 
axis, are used to uniquely define the coordinate <p. The algebraic relationship 
between the coordinates (xi, X2, xz) of the point Ps and the coordinates (a, (p, yA 
of the same point Pt is defined by the following equation. 



Cart.->Sph. 
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atan(jc2,jc,) 
atan(x3,^x,^4-X2 ) 



atan(^2»^i) 
sgn(^3)^ 



for 56 0 or ^2 0 



for = 0 and a:2 = 0 



Similarly, the algebraic relationship between the coordinates (v^, vz, vz) of a 

vector V5 specified in the Cartesian system and the coordinates (w, v^, of the 

same vector \t specified in the spherical system at the point Ps is defined by the 
following equation. 



= g 



Cart.-^Sph 



V, 008^008^^/- + V2 sin ^ cos ^i/^ + V3 

-Vj sin^ + V2 cos^ 
- Vj cos ^ sin ^ - V2 sin ^ sin + V3 cos xj/ 
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i 



xf +xl +xl 



- (vi^i + V2X2 k + V3 + 4 ) 



^(Vl^l+V2g2)g3+V3fe+^2) 



4vf+V2)+V3^3 



0 



0 
0 



for X, ^ 0 or ^2 5^ 0 



forx, =0,^2 =0,qr, =;ivj, 
and ^2 ^ >^'V2 



for = 0, ^2 = 0, = Xv^, 



forx, =0,^2 =0,^j =Avi, 
^2=^V2,and93=;iv3 



4. Other Coordinate Systems 

[0087] Additional and/or different coordinate systems may also be used. For 
example, it may be desirable to develop one or more other coordinate systems 
which are tailored to be used in specific applications. Two such coordinate 
systems are shown in FIGS. 13-14. In FIG. 13, a coordinate system 140 is 
shown which is useable in the context of a two-arm robotics application in 
which the two arms are independently moveable ("independently linked"). In 
FIG. 14, a coordinate system 150 is shown which is useable in the context of a 
two-arm robotics application in which the two arms are not independently 
moveable ("dependently linked"). 
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a. Independent Coordinate System 

[0088] Referring first to FIG. 13, the geometric relationship of a left-hand 
< 0) independent coordinate system is shown. An independent coordinate 
system is a kinematic system with two independent links. The coordinate origin 
O, the length of the two links (0 < h and 0 < h), and the two coordinate curves 
|ai,a2} define an independent coordinate system. With the same linear scale of 
measurement on the two links and a radian scale of measurement on the curves 

a\ and aa, the independent coordinates ai) define a point P in independent 
coordinate space. 

[0089] A transformation may be used to transform coordinates in the 
independent coordinate system to coordinates in the Cartesian coordinate 
system. This transformation is sometimes referred to as forward kinematics. 
The algebraic relationship between the coordinates (ai, ai) of a point Ps specified 
in the independent system and the coordinates (x\, xi) of the same point Pt 
specified in the Cartesian system is defined by the following equation. 

Indep.->Cart. \^ s / 

/j cosa^ +1^ cos(a, +^2) 
/, sin Gfj + sin(a, ^a^) 

[0090] A transformation may also be used to transform coordinates in the 
Cartesian coordinate system to coordinates in the independent coordinate 
system. This transformation is sometimes referred to as inverse kinematics. 
Since the kinematic system can either represent a left-hand (ai < 0) or a right- 
hand (0 < ai) system, the result of this transformation is not uniquely defined. 
However, by establishing one configuration during initialization, the result is 
uniquely defined from then on. 

[0091] With the polar coordinates of a point Ps 
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r 




JP. 





atan(A:2,JCi) 



and the sine and cosine rule, the angles >0and ;^can be derived. First, the cosine 
rule 



provides the angle 



= acos 



Second, the sine rule 



sin 



provides the angle 



asm 
0 



(^2 ■ 

— sin;K 



for r 0 
forr = 0 



With the above results, the algebraic relationship between the coordinates 
X2) of a point Ps specified in the Cartesian system and the coordinates {a^, 02) of 
the same point Pt specified in the independent system is defined by the following 
equations. 



Left-hand system: 



Right-hand system: 



= f. 



Cart.-*Indep. 



a, 






«2. 







= f, 



Cart.->Indep 
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'(P-P 


«2. 




71 -y 



b. Dependent Coordinate System 

[0092] Referring next to FIG. 14, the geometric relationship of a left-hand {ai 
< a^) dependent coordinate system is shown. A dependent coordinate system 
is a kinematic system with two dependent links. The coordinate origin O, the 
length of the two links (0 < /i and 0 < h), and the two coordinate curves 
^i,a2} define a dependent coordinate system. With the same linear scale of 
measurement on the two links and a radian scale of measurement on the curves 

a\ and ai, the dependent coordinates (a^, 02) define a point Pin dependent 
coordinate space. 

[0093] A transformation may be used to transform coordinates in the 
dependent coordinate system to coordinates in the Cartesian coordinate system. 
This transformation is sometimes referred to as forward kinematics. The 
algebraic relationship between the coordinates 02) of a point Ps specified in 
the dependent system and the coordinates (xi, xi) of the same point A specified 
in the Cartesian system is defined by the following equation. 

Pt = foep.-^CartX^) 

/j cos a, -1-/2 cosoTj 
/, sinaj +I2 sina2 _ 

[0094] A transformation may also be used to transform coordinates in the 
Cartesian coordinate system to coordinates in the dependent coordinate system. 
This transformation is sometimes referred to as inverse kinematics. Since the 
kinematic system can either represent a left-hand (02 < a^) or a right-hand {a^ < 
02) system, the result of this transformation is not uniquely defined. However, 
by establishing one configuration during initialization, the result is uniquely 
defined from then on. 
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With the polar coordinates of a point Ps 



r 




JP. 





atan(jC2,Jc,) 



and the sine and cosine rule, the angles /?and ;^can be derived. First, the cosine 
rule 



-21^2 cos/ 



provides the angle 



= acos 



Second, the sine rule 



sin p 



sinx 



provides the angle 



P = 



. (I2 , ^ 
asm —smy 

0 



for r 9^ 0 
forr = 0 



With the above results, the algebraic relationship between the coordinates (xi, 
xi) of a point Ps specified in the Cartesian system and the coordinates {a^, ai) of 
the same point Pt specified in the dependent system is defined by the following 
equations. 

Left-hand system: 



— fcart.->Dep. ) 
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a 



<P + P 
<p + p + y-7T 



Right-hand system: 



or, 



= f. 



Cart .-►Dep. (p.) 
(p- P-^-K-y 



B. Coordinated Move Profiles 

[0095] In the preferred embodiment, the coordinated move block 72 may be 
used to provide, for example, a linear or circular path in a Cartesian coordinate 
system from the current position to a new position. For circular moves, the path 
also either takes into account an auxiliary position that specifies the center of 
the circle or a position along the circle, or alternately the radius of the circle may 
be specified in place of an auxiliary position. The actual path over time also 
depends on the current speed and acceleration, the maximum speed, 
acceleration, and deceleration as well as the velocity profile type. 

1 . Linear Move 

[0096] Referring now to FIG. 1 5, a coordinated linear move provides a linear 
path in a multi-dimensional Cartesian coordinate system from a current position 
Pc to a new position Pn, FIG. 15 shows the geometric relationship of a simple 
two-dimensional linear move. In general, the ideal line for this move is defined 
by the old position Po, which specifies either the position of a previous 
coordinated move or the current position Pc. If there was no previous 
coordinated move, the old position Po is assumed to be equal to the current 
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position Pc. Since the actual path generally is not exactly linear due to the 
current position Pc, the current velocity Vc , and the current acceleration Uc, the 
resulting path is fairly complex. However, the representation of the path can be 
simplified by transforming the move parameters from the original (target) 
Cartesian coordinate system to a new (source) Cartesian coordinate system. An 
appropriate source system is defined by the origin Os, which is equal to the old 

position Po, and the coordinate axes where n denotes the number 

of coordinated axes. 



[0097] The coordinate axes pcu, jc„,5 1, are characterized by the mutually 
orthonormal basis which may be defined by the following procedure. 



1. The first m basis vectors tfi, can be calculated by applying the 



auxiliary algorithm of Section 11(C), where the vectors |^i,- -,rm| specify the 
first m {m < n) mutually independent vectors in the set Pn - Po, Pn - Pc, Vc , 

and flc. The algorithm in Section 11(C) also handles the identification of the 
first m mutually independent vectors. 



2. The remaining n - m basis vectors Vm+i, --,^«| can be calculated by applying 



the auxiliary algorithm of Section 11(C), 

[0098] The resulting offset vector o - Os - Ot and transformation matrix = 
r = ^i, •••,/„!, define the source coordinate system. Therefore, the current 
position Pc, the new position Pn, the current velocity Vc , and the current 
acceleration Uc can be transformed from the target to the source system. 








Pn,s 
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[0099] In the source system, the multi-dimensional coordinated linear move 
can be separated into multiple individual one-dimensional moves (mi ... mn). 

Therefore, the coordinates of the velocity Vs and acceleration as represent the 

start velocity and start acceleration for the individual moves. The distance ds for 
all individual moves specified in the source system can be calculated based on 
the current position Pc^s and the new position Pn^s specified in the source system. 

Pn,s — Pes 

(Pn - Pc) 

[0100] During run-time, the interpolator of each individual move block 76 
generates incremental position reference values for the coordinates of Ps in the 
source system. With these updates, the incremental position reference values Pt 
in the target system can simply be calculated by applying a coordinate 
transformation from the source to the target system. 

Px^TPs+O 

2, Circular Move 

[0101] Referring now to FIG. 16, a coordinated circular move provides a 
circular path in a two- or three-dimensional Cartesian coordinate system from a 
current position Pc to a new position Pn, FIG. 16 shows the geometric 
relationship of a simple two-dimensional circular move. In general, the ideal 
circle for this move is defined by the old position Po, which specifies either the 
position of the previous coordinated move or the current position Pc, and the 
auxiliary position Pa, which specifies either the center of the circle or a position 
along the circle. (Alternatively, the radius may be specified from which the 
center is calculated.) If there was no previous coordinated move, the old 
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position Po is assumed to be equal to the current position Pc. Since the actual 
path generally is not exactly circular due to the current position Pc, the current 
velocity Vc , and the current acceleration Uc, the resulting path Is more complex. 
However, the representation of the path can be simplified by transforming the 
move parameters from the original (target) Cartesian coordinate system to a new 
(source) cylindrical coordinate system. An appropriate source system is defined 
by the origin Os, which is equal to the center of the circle, and the coordinate 
axes {xi,^,- -,:^^,^}, where n denotes the number of coordinated axes. However, 

in order to calculate the origin Os, the vector n that is normal to the plane of the 
circle needs to be calculated first. 

[0102] In the general case that the vectors Pn - Pa and Pa - Po are linearly 
independent and an auxiliary vector a is not specified, the normal vector n is 
defined by the outer product of the vectors Pn - Pa and Pa - Po. 

b = {P„-PaHPa-Po) 

b\ 

However, if the vectors Pn - Pa and Pa - Po are linearly dependent and an 
auxiliary vector a is not specified, the circle is not defined. 

[0103] With the positions Pa and Po on the plane of the circle the 
corresponding projected positions Pa' and Po' onto the plane of the circle are 
equal to the original positions. 

F = P 

a a 

F = P 

o ^ o 

In the special case that an auxiliary vector a is specified, the normal vector n is 
defined by the auxiliary vector a . 
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||a 

However, if the auxiliary vector a is zero, the circle is not defined. 

[0104] With the positions Pa and Po not necessarily on the plane of the circle, 
the projected positions Pa' and Po' onto the plane of the circle are calculated. 
Since a projected position P has to be on a straight line, which goes through the 

original position P and is parallel to the vector n , 

P' = P^Xn 

and has to be on the plane, which goes through the new position Pn and is 
normal to the vector n , 

-n\P-P„) = 0 
the projected position P Is defined by the following equation. 

'n[p-\-Xn-P„) = 0 

Xn^n = n^{P„-P) 

X = -n{P„-P) 

F = P + i^'{p^-p)}t 

As a result, the projected positions Pa' and Po' are defined by the following 
equations. 
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If the auxiliary position Pa specifies the center of the circle, the origin Os is 
directly defined. 

If the auxiliary position Pa specifies a position along the circle, the origin Os is 
defined by the intersection of the following three planes. 

{K-P'Jo, = {K-KJ^ 

n O, = n P„ 

[0105] The first plane passes through the mid-point between Pn and Pa and is 
orthogonal to Pn - Pe . The second plane passes through the mid-point between 
Pa' and Po and is orthogonal to Pa - Po'. The third plane passes through the 
positions Pn, Pa', and Po'. The origin Os can be found by solving the above set of 
three linear equations. 

However, if the vectors Pn - Pa and Pa' - Po are linearly dependent, the circle is 
not defined. 

For a two-dimensional circular move, the third plane is defined implicitly and the 
problem degenerates to a set of two linear equations. 

[0106] The coordinate axes \x\,s,---,Xn,, } are characterized by the mutually 
orthonormal basis ^i,---~t„}. which is defined by the following procedure. 

1 . With n representing the first non-zero vector in the set Po' - Os and Pn - Os, 
the first basis vector 
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is defined. 

2. Witii the normal vector n , the second basis vector 

t2 = nxt\ 

is defined. For a two-dimensional circular move, the normal vector is defined 
implicitly and the problem degenerates. 



t2 




3. The third basis vector ti is equal to the normal vector n . 

10107] The resulting offset vector o = Os - Ot and transformation matrix 

... f I define not only the source cylindrical coordinate system but also 
an intermediate Cartesian coordinate system. Therefore, the current position Pc, 
the new position Pn, the current velocity Vc , and the current acceleration ac can 
be first transformed from the target to the intermediate system. 



Pa 


— T 

= T 


[p. 




— T 




Pn,l 


= T 


[p. 


V,- 


— T 

= T 






— T 

= T 





[0108] In the special case that the position Pa is located on the xi,, -axis at 
least one of the vectors in the set v,,a, , and Pn.i - Pa, has to be non-parallel to 
the jc3,, -axis. The first non-parallel vector in the above set defines the vector q.,. 
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Therefore, the current position Pc,/, the new position Pnj, the current velocity v, , 
and the current acceleration at can be transformed from the intermediate to the 
source system. 

Pes = fcart.->Cyl.(^c,i'9j ) 

Pn,s = fcart.^Cyl.(^«./>^i) 

Vs = gcart.->CyK(^c,/'9/5^i) 

as — 8cart.^Cyl.(^c.i>^|9^/ ) 



[0109] In the source system, the two- or three-dimensional coordinated 
circular move can be separated into two or three individual one-dimensional 
moves (mi, ma, and ma). Therefore, the coordinates of the velocity Vs and 
acceleration as represent the start velocity and start acceleration for the 
individual moves. The distance ds for all individual moves specified in the source 
system can be calculated based on the current position Pes and the new position 
Pas specified in the source system. 



[01 10] Since the old position Po and the new position Pn split the circle into 
two arcs, the direction of the path along the circle still needs to be defined. 
Therefore, the coordinate (pa of ds may need to be adjusted depending on the 
direction of the path. 



[0111] If an auxiliary vector a is specified, the right-handed screw rule 
specifies the direction of the path. With the coordinate q)c and <pn of the positions 
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Pes and Pn.s the coordinate q)d of ds can be adjusted according to the following 
algorithm. 



(pd = (pn — (pc 

if ^ay < 0 

^ + = 27U 

[01 12] If an auxiliary vector a is not specified, the old position Po needs to be 
transformed from the target to the source system. 



[01 13] If an auxiliary vector a is not specified and the auxiliary position Pa 
specifies the center of the circle, the shorter arc defines the direction of the 
path. With the coordinates <po, <pc, and (pn of the positions Po.s, Pc^s, and Pn^s the 

coordinate ^ of ds can be adjusted according to the following algorithm. 

if ^ < ^ 

if ^ - ^ < 71 

direction = positive 

else 

direction = negative 

else 

if ^ - ^ < 71 

direction = negative 

else 

direction = positive 

^ = (pn - (pc 
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if direction = = positive 
if «ay < 0 

qjd + = 2n 

else 

if 0 < 9w 

gjd 2n 

[0114] If an auxiliary vector a is not specified and the auxiliary position Pa 
specifies a position along the circle, the arc, which includes the auxiliary position 
Pa, defines the direction of the path. Therefore, the auxiliary position Pa needs to 
be transfornned from the target to the source system. 

Paj 
Pa,s 

[01151 With the coordinates ^, (pc, (pa, and qh of the positions Po,s, Pes, Pa,s, 
and Pn.s the coordinate <pd of ds can be adjusted according to the following 
algorithm. 

\f <po < q>a and qja < <pn 

direction = positive 

else 

direction = negative 

else 

\f <pn < and (pa < qh 

direction = negative 

else 

direction = positive 



= t"^ {Pa' - o ] 



= f . 



Cart.->Cyl 
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(pd = (fh - q>c 

if direction = = positive 

if ^^^y < 0 

^ -h = 27C 

else 

if 0 < ^ 

^ - = 27C 

[0116] During run-time each individual move generates position updates for 
the coordinates of Ps in the source system. With these updates, the position 
updates Pt in the target system can simply be calculated by applying a 
coordinate transformation from the source to the target system. 

3. Blended Move 

[01 17] Referring now to FIG. 1 7, in general, the transition from one move to 
the next move simply involves using the dynamics of the current move as start 
dynamics for the next move. However, this might not generate the desired path 
in all circumstances. An example would be a straight transition from a linear 
move to a circular move. In one embodiment, the coordinated move block 72 
causes a transitioning path segment to be generated which approaches the circle 
quickly instead of continuing the linear move until the transition position Po, 

[01 18] In the preferred embodiment, since this behavior is not always desired, 
the concept of blended moves is also supported. This blending method provides 
transitioning at speed from the current move to the next move at or before the 
transition position Po. The resulting path is shown in FIG. 17. 

[01 19] The blending algorithm preferably comprises the following: 

1 . Calculate the distance d along the ideal path from the old position Po to the 
new position Pn. Based on the type of the next move this can be a linear or 
circular path. 
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2. Scale the distance d with the scaling value s which is based on the tangential 
vectors to and t„ . 

to t n 

fr for r > 0 
0 forr<0 

d' = sd 

3. Initiate an incremental move along the ideal path of the current move of 
distance d'. Based on the type of the current move this can be a linear or 
circular move. 

4. Transition to the next move at or before the old position Po based on the 
specified transition range. 

Due to the general approach of this algorithm, any combination of linear and 
circular move can be blended. 

C. Auxiliary Algorithms 

[0120] The two auxiliary algorithms described in this subsection may be used 
to calculate the transformation matrices for the coordinate transformations and 
coordinated moves. 

1 . Transformation to Orthonormal Basis Vectors 

[0121] Given the set of basis vectors {ri, -sr^t} in n-dimensional space, this 
algorithm calculates the corresponding mutually orthonormal basis vectors 

1, --s/m} using the Gram-Schmidt prthogonalization method. Even though this 
method is not as numerically reliable as the singular value or QR decomposition 
method, the number of required mathematical operations for the typical number 
of basis vectors is smaller. 
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[0122] Initially, all vectors in the set |ri,--,r;t) that are zero are removed 
from the set. Then, the first orthonormal basis vector 



t\ 




is defined. The remaining orthonormal basis vectors j^2. --Jm] are calculated 
iteratively. First, the basis vector 




is calculated. If the basis vector r/ is linearly dependent to the basis vectors 
fi, \ the resulting vector Si is zero and both n and 5, are removed from 

the set. Otherwise, the basis vector Si is orthogonal to the basis vectors 
^1, '",ti-\\. Second, the corresponding orthonormal basis vector 



ti 




is defined. 



2. Complementation of Orthonormal Basis Vectors 

[0123] Given the mutually orthonormal basis vectors ^\,-'Jm \ in n- 
dimensional space, this algorithm calculates the complementing mutually 
orthonormal basis vectors ^m+i, ",//i}. 

[0124] The first n - m - ^ basis vectors ^m+i, - s^/i-i } are calculated 
iteratively. First, the basis vector 
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-T 
Si 



is defined by the cofactors vj that correspond to the element to of the 
transformation submatrix 



^il 



Si hi. 



If the resulting vector 5/ is zero, it can not be used as basis vector and is 
replaced with 



-T 
Si 



0 0 1 0 0 



(-1 



Due to the above definitions and the following equation 



si'tj = ki ^ii 0 0 



hi 



T yt H VT-t- 

i\ j\ II ji 



= det 
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the basis vector Si satisfies the following condition. 



[0125] Therefore, the basis vector Si is orthogonal to the basis vectors 
Second, the corresponding orthonormal basis vector 




is defined. The calculation of the last orthonormal basis vector tn can be 
simplified, since the transformation matrix r = L, is orthogonal. 





-T ' 
ti 








-T 
tn 




... tn 




'-T- 

tx t 


i 


-T- 
' tl tn 




-T- 
tnt 


1 


-T- 
' tntn 




1 







[0126] This orthogonality equation can be used to determine the determinant 
of the transformation matrix T . 



— T — 

T T 



- / 
= 1 



detr^ detr 



= 1 



-49- 



Atty. Dkt. No.: 02AB200 (081696-0251) 



(detrf = 1 

detr = 1 

[0127] In addition, the orthogonality equation can be restated using the above 
result, Cramer's rule, and the cofactors z7/that correspond to the element tij of 
the transformation matrix T . 



— T 

T 



detr 



[0128] Therefore, the last orthonormal basis vector t„ is defined by the 
following equation. 



III. Instructions 

[0129] Referring now to FIGS. 18-24, as mentioned above, motion control 
operations including move operations are achieved through the use of one or 
more multi-axis coordination instructions 31 , The multi-axis coordination 
instructions 31 are part of the same programming language (e.g., a ladder logic 
programming language) as the instructions used in connection with the blocks 
shown in FIG. 2. Assuming an object-orientated approach is used, the multi-axis 
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coordination instructions 31 invoke the services of the coordinated move block 
72 and the coordinate transfornnation block 74. 

[0130] FIGURES 18-22 show the multi-axis instructions 31 as they appear to 
the programmer in one embodiment in which the instructions are instructions in 
a ladder logic programming language. The axes representing a Cartesian 
coordinate system can be moved to a new position using a coordinated move 
instruction 200 (FIG. 18), and can be issued on the fly path dynamic changes 
using a change coordinated dynamics instruction 500 (FIG. 21). In addition, the 
axes representing a target coordinate system can be locked to the axes 
representing a source coordinate system using the coordinate transformation 
instruction 300 (FIG. 19). Further, a position specified in the source coordinate 
system can be transformed into the same position specified in the target 
coordinate system using the calculate target position instruction 400 (FIG. 19). 
The axes involved in a coordinate system may be stopped together using the 
coordinated stop instruction 600 (FIG. 22), All instructions are part of the group 
of motion move instructions. FIGURES 23-24 show alternative embodiments of 
the coordinated move instruction 200. 

A. Coordinated Move Instruction 200 

[0131] Referring now to FIG. 18, in a first embodiment, a linear or circular 
coordinated move for the specified motion control axes within a Cartesian 
coordinate system may be initiated using a coordinated move instruction 200. 
FIGURE 18 shows an exemplary ladder faceplate of the coordinated move 
instruction 200. A linear move approaches the new position directly. A circular 
move approaches the new position by circling either around or via the auxiliary 
position. The new and auxiliary positions can be defined as absolute or 
incremental position. 

[0132] The coordinated move instruction 200 has operands 210-244 as 
shown in FIG. 18. As described below, the operands 210-244 are used to 
specify user-configurable parameters that configure the coordinated move 
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initiated by execution of the coordinated move instruction 200. These operands 
will now be described in greater detail. 

[0133] The coordinate system operand 210 specifies the group of motion 
control axes which span the coordinates (xi, Xn) of a Cartesian coordinate 
system. The group of motion control axes correspond to axes of movement as 
defined by physical hardware (e.g., motors 27 and/or related hardware). The 
coordinate system may support up to thirty-two axes or more, depending on 
system configuration. 

[0134] The motion control operand 212 provides information via bits 213. 
The bits 213 include an enable bit (EN) which is set when the rung transitions 
from false to true and which is reset when the rung transitions from true to 
false, a done bit (DN) which is reset when the rung transitions from false to true 
and which is set when the coordinated move has been initiated successfully, an 
error bit (ER) which is reset when the rung transitions from false to true and 
which is set when the coordinated move has not been initiated successfully, an 
in process bit (IP) which is set when the coordinated move has been initiated 
successfully and which is reset when the coordinated move reaches the new 
position or when the coordinated move is superseded by another coordinated 
move instruction or terminated by some other motion instruction, and a process 
complete bit (PC) which is reset when the rung transitions from false to true and 
which is set when the coordinated move reaches the new position. In addition 
to providing information via the bits 213, the coordinated move instruction 200 
may also have other indicators such as error code indicators. 

[0135] The move type operand 214 defines the path type of the coordinated 
move. In one embodiment, the coordinated move instruction 200 may be used 
for linear or circular move profiles. In this embodiment, the move type may be 
set to absolute linear (the axes move directly to the new position, which is 
defined by the position array), incremental linear (the axes move directly to the 
new position, which is defined by the sum of the position array and the old 
position), absolute circular (the axes move along a circular path to the new 
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position, which is defined by the position array), or incremental circular (the axes 
move along a circular path to the new position, which is defined by the sum of 
the position array and the old position). Depending on the selected transition 
type operand 238, the current position or the position of the previous 
coordinated move defines the old position. 

[0136] The position operand 216 defines either the new absolute or 
incremental position, depending on the value of the move type operand 214. The 
position operand 216 is a one-dimensional array, whose dimension is defined by 
the number of specified axes. 

[0137] The auxiliary type operand 218 specifies how the auxiliary position 
operand 220 defines the circle if the move type operand 214 is set to circular. 
(If the move type operand 214 is set to linear, the specification of the auxiliary 
type is irrelevant/not used.) In one embodiment, the auxiliary type operand 218 
may be set to absolute center (the auxiliary position array defines the center of 
the circle), incremental center (the sum of the auxiliary position array and the old 
position defines the center of the circle), absolute via (the auxiliary position array 
defines a position along the circle), or incremental via (the sum of the auxiliary 
position array and the old position defines a position along the circle). Depending 
on the value of the transition type operand 238, the current position or the 
position of the previous coordinated move defines the old position. 

[0138] The auxiliary position operand 220 defines either the absolute or 
incremental auxiliary position, depending on the value of the auxiliary type 
operand 218. The auxiliary position operand 220 is a one-dimensional array, 
whose dimension is defined by the number of specified axes. If the move type 
operand 214 is set to linear, the specification of the auxiliary position operand 
220 is optional. However, if the move type operand 214 is set to circular, the 
auxiliary position operand 220 specifies the center of the circle or a position 
along the circle. 

[0139] The auxiliary vector operand 222 defines a vector that is normal to the 
plane of the circle if the move type operand 214 is set to circular. If the auxiliary 
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type operand 218 is set to center, the direction of the auxiliary vector specifies 
the rotational direction of the circular move according to the right-handed screw 
rule. The auxiliary vector operand 222 is a one-dimensional array, whose 
dimension is defined by the number of specified axes. In general, the 
specification of the auxiliary vector operand 222 is optional. However, the 
auxiliary vector operand 214 may be specified when the move type operand 214 
is set to circular and the vectors from the old position to the auxiliary position 
and from the auxiliary position to the new position are linearly dependent. The 
auxiliary vector operand 214 may also be specified when the move type operand 
214 is set to circular, the auxiliary type operand 218 is set to center, and the 
rotational direction should not follow the shorter arc. 

[0140] The speed operand 224, accel rate operand 228, and decel rate 
operand 232 define the maximum speed, acceleration, and deceleration along 
the path of the coordinated move. The speed units operand 226, accel units 
operand 230, and decel units operand 234 define the units that are applied to 
the values of speed, accel rate, and decel rate, where the maxima represent the 
maxima of the first coordinate system and the units represent coordination units. 
The profile operand 236 specifies whether the velocity profile of the coordinated 
move follows a trapezoidal or S-curve profile. 

[0141] The transition type operand 238 defines the old position, which is used 
to establish the ideal path to the new position, and the transition schedule, 
which defines when the coordinated axes will start transitioning to the new 
position. The transition type operand 238 may be set to various values as 
indicated below. 



Transition 
Type 


Behavior 


Direct 


The current position defines the old position. 
The coordinated axes immediately start moving to 
the new position. 


Immediat 


The position of the previous coordinated move 
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e 


defines the old position. If there was no previous 
coordinated move, the old position is assunned to be 
equal to the current position. 

The coordinated axes immediately start transitioning 
to the new position. 


Decel 


The position of the previous coordinated move 
defines the old position. If there was no previous 
coordinated move, the old position is assumed to be 
equal to the current position. 
When the previous coordinated move starts to 
decelerate, the coordinated axes start transitioning 
to the new position. 


Position 


The position of the previous coordinated move 
defines the old position. If there was no previous 
coordinated move, the old position is assumed to be 
equal to the current position. 
When the remaining position of the previous 
coordinated move is less than the transition range, 
the coordinated axes start transitioning to the new 
position. 


Position 
and 

Blended 


The position of the previous coordinated move 
defines the old position. If there was no previous 
coordinated move, the old position is assumed to be 
equal to the current position. 
When the remaining position of the previous 
coordinated move is less than the transition range, 
the coordinated axes start transitioning to the new 
position along a blended path. 



[0142] The merge operand 242 specifies whether or not to turn either all the 
motion or just the coordinated portion of the move into a pure coordinated move 
for all axes in the coordinated system: 
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Merge 


Behavior 


Disabled 


Any currently executing single axis nnotion 
instructions involving any axes defined in the 
specified coordinate system will not be affected 
by the activation of this instruction, and will result 
in superimposed motion on the affected axes. 
Also, any coordinated motion instructions 
involving the same specified coordinate system 
will run to completion based on its termination 
type. 


Coordinate 
d Motion 


Any currently executing coordinated motion 
instructions involving the same specified 
coordinate system will be terminated, and the 
prior motion will be merged into the current move 
at the speed defined in the merge speed 
parameter. Also, any pending coordinated motion 
instructions will be cancelled. Any currently 
executing system single axis motion instructions 
involving any axes defined in the specified 
coordinate system will not be affected by the 
activation of this instruction, and will result in 
superimposed motion on the affected axes 


All Motion 


Any currently executing single axis motion 
instructions involving any axes defined in the 
specified coordinate system, and any currently 
executing coordinated motion instructions 
involving the same specified coordinate system, 
will be terminated, and the prior motion will be 
merged into the current move at the speed 
defined in the merge speed parameter. Also, any 
pending coordinated motion instructions will be 
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cancelled. 



If the merge is enabled, the merge speed operand 244 defines the maximum 
speed along the path of the coordinated move. 

B. Coordinate Transformation Instruction 300 

[0143] The coordinate transformation 300 instruction initiates a coordinate 
transformation between the specified source and target coordinate system axes. 
Based on the type of the source and target system and the specified parameters, 
some or all of the following steps are performed: Transformation of the source 
system to a Cartesian coordinate system; rotation of the Cartesian coordinate 
system with the rotation matrix R ; translation of the Cartesian coordinate 
system with the offset vector o ; and/or transformation of the Cartesian 

coordinate system to the target system. The transformation matrix f can be 
calculated from the rotation matrix R ; by applying the auxiliary algorithm of 
Section il(C)(2). Therefore, the algebraic relationship between the source and 
target system is defined by: 



Before executing the coordinate transformation instruction 300, the target axes 
may be moved to the position that corresponds to the position of the source 
axes and the specific coordinate transformation. The user can calculate the 
required position of the target axes using the calculate target position instruction 



[0144] The ladder faceplate of a three-dimensional coordinate transformation 
instruction 300 is shown in FIG. 19. The coordinate transformation instruction 
300 has operands 310-320. The target and source coordinate system operands 
310 and 312 specify the reference frames used to describe a set of individual 
axis positions. The following table shows the relevant coordinate systems, their 
dimension (n), and the notation used to describe the individual axis positions. 



Pt 



= f . 




Source ^Cart. 




400. 
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The coordinate transformation instruction 300 supports transformations to and 
from each of the following coordinate systems.: Cartesian, articulated cylindrical, 
articulated independent (e.g., 2, 3, or 4 axes), articulated dependent (e.g., 2, 3, 
or 4 axes), polary/cylindrical, SCARA independent, SCARA dependent, 
spherical. (A SCARA: (Selective Compliance Assembly Robot Arm) coordinate 
system is used for a combination of cylindrical and revolute configurations 
operating in the horizontal plane. This arrangement includes three linked arms 
with two rotary joints to provide movement in the horizontal plane; vertical 
movement is provided at the end of the arm.) Other coordinate system 
transformations may also be supported. 

[0145] The maximum number of target or source axes defines the actual 
dimension of the coordinate transformation. If the number of target axes is 
higher than the number of the source axes, the coordinates for the missing 
source axes are assumed to be zero. In the case of an independent or 
dependent system, the coordinate transformation instruction 300 establishes a 
left-hand or right-hand configuration based on the initial angular coordinates. 
Thereafter, the established configuration is maintained. Hence, the coordinate 
transformation instruction 300 provides multi-dimensional coordinate 
transformations with the maximum dimension dependent on the selected source 
and target system. 

[0146] For a linear coordinate, the coordinate value represents coordination 
units, which are defined by the coordination constant. For an angular coordinate 
such as (p, y/, ai, or ca, the coordinate value represents radians. 

[0147] The target and source coordinate systems preferably support up to n 
axes (e.g., n = 32, depending on system configuration). Note that for some 
transformations the target and source systems may contain the same axes. 

[0148] The motion control operand 314 provides Information via bits 315 
which operate in generally the same manner as the bits 213 of FIG. 18. In 
addition to providing information via the bits 315, the coordinate transformation 
instruction 300 may also have other indicators such as error code indicators. 
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[0149] The rotation matrix R operand 316 defines the transformation matrix 
that is used to rotate the Cartesian coordinate system. It is a two-dimensional 
array, whose dimensions are defined by the maximum number of specified target 
or source axes. However, the specification of the rotation matrix is optional. If 
the rotation matrix is not specified it is assumed to be a unit matrix. 

[0150] The offset vector o operand 318 defines the translation of the 
Cartesian coordinate system. It is a one-dimensional array, whose dimension is 
defined by maximum number of specified target or source axes. However, the 
specification of the offset vector is optional. If the offset vector is not specified 
it is assumed to be zero. 

[0151] The master reference operand 320 specifies whether the actual or 
command position of the source axes should be transformed. The transformation 
may be bidirectional. Transformation preferably may however only occur in one 
direction at a time. The direction of transfer is only dependent on which side 
(source or target) the command is being generated. 

C. Other Instructions 

[0152] FIGURES 20-22 show other instructions that may be employed in 
connection with one or both of the coordinated move block 72 and the 
coordinate transformation block 74. These instructions include a calculate target 
position instruction 400, a change coordinated dynamics instruction 500, and 
axis coordinated stop instruction 600. 

[0153] The calculate target position instruction 400 transforms a specified 
position from the source coordinate system into the target coordinate system. 
The calculate target position instruction 400 provides multi-dimensional 
coordinate transformations with the maximum dimension dependent on the 
selected source and target system. Based on the type of the source and target 
system and the specified parameters, some or all of the following steps are 
performed: transformation of the source system to a Cartesian coordinate 

system; rotation of the Cartesian coordinate system with the rotation matrix R ; 
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translation of the Cartesian coordinate systenn with the offset vector o ; and/or 
transformation of the Cartesian coordinate system to the target system. 
Therefore, the algebraic relationship between the source and target system is 
defined by: 



The ladder faceplate of a calculate target position instruction 400 is shown in 



[0154] The change coordinated dynamics instruction 500 initiates a change in 
dynamics for the specified coordinate system motion. Based on the specified 
motion type, the change coordinated dynamics instruction 500 changes the 
coordinated move (or jog) motion profile active on the specified system. In case 
of a coordinated move, the change coordinated dynamics instruction 500 
changes the dynamics for the active as well as the pending coordinate move. 
The ladder faceplate of the change coordinated dynamics instruction 500 is 
shown in the FIG. 21 . 

[0155] The axis coordinated stop instruction 600 initiates a controlled stop of 
the specified coordinate system motion or transformation. Based on the specified 
stop type, the axis coordinated stop instruction 600 stops either all motion 
profiles or only a specific motion profile of the specified coordinate system. The 
ladder faceplate of the axis coordinated stop instruction 600 is shown in 



[0156] FIGURES 23-24 show an alternative embodiment of the coordinated 
move instruction 200. In the embodiment as shown in FIGS. 23-24, separate 
instructions are provided for linear moves and for circular moves . 

[0157] The major differences between these two embodiments will now be 
described in greater detail. Since a separate instruction is used in this 
embodiment for linear and circular, the instruction used defines the type of move 
to be performed, either linear or circular. 



Cart .-►Target 




FIG. 20. 



FIG. 22. 
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[0158] Figure 23 shows a linear instruction in accordance with this 
embodiment. The only two parameters that are different from the previous 
embodiment for linear are move type 500 and termination type 502. The move 
type 500 is used to define if the move is absolute or incremental. The definition 
of absolute and incremental is the same as the previous embodiment. 



[0159] The termination type 502 is defined as follows: 



Transition 
Type 


Behavior 


Actual 
Tolerance 


When the vector position lag for the primary axes 
and the position lag for each ancillary axis is less 
than the actual tolerance, the instruction will 
terminate. The position lag is the actual position 
minus the commanded position and is computed 
after the entire linear move has been interpolated, 
that is, when the distance to go equals zero. A 
possible following instruction can then begin 


No Settle 


When the vector position lag for the primary axes 
and the position lag for each ancillary axis is less 
than the actual tolerance, the instruction will 
terminate. The position lag is the actual position 
minus the commanded position and is computed 
after the entire linear move has been interpolated, 
that is, when the distance to go equals zero. A 
possible following instruction can then begin. 


Command 
Tolerance 


When the vector distance-to-go for the primary axes 
and the distance-to-go for each ancillary axis is less 
than the command tolerance, the instruction will 
terminate. The distance-to-go is the distance from 
the programmed endpoint to the commanded 
position. A possible following instruction can then 
begin. If there is no following instruction, the move 
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will degenerate to No Settle. 


No Decel 


When the entire linear move has been interpolated 
up to the deceleration point, the instruction will 
terminate. The deceleration point of the move is 
dependent on the profile chosen {trapezoidal or S- 
curve). A possible following instruction can then 
begin. If there is no following instruction, the move 
will degenerate to No Settle. 



[0160] In addition to all the bits defined for the previous embodiment, this 
embodiment has an (AC) bit which is set when the instruction is being executed. 
There is a queue of instructions which have been partially processed. The AC bit 
indicates which of the instructions is being executed. 

[0161] Figure 23 shows a circular instruction for this embodiment. The 
following five operands are different from the previous embodiment: move type 
504, circle type 505, via/center/radius 506, direction 507 and termination type 
508. 

[0162] The move type operand 504 is defined as described above in 
connection with the move type operand 500. The circle type operand 505 
describes the type of circle being programmed. It defines how to interpret the 
via/center/radius operand 506. The circle type operand 505 specifies if the value 
given in the operand 506 should be used as a point along the arc of the circle or 
the circle center, or the circle radius. 

[0163] The via/center/radius operand 506 defines an array which is interpreted 
as defined by the operand 505, i.e., a point on the circle as an absolute via (the 
array value defines a position along the circle), or an incremental via (the sum of 
the array value and the old position defines a position along the circle) or the 
absolute center or an incremental center (the sum of the array value and the old 
position defines the center of the circle), or the radius of the circle. 
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[0164] The direction operand 507 specifies the rotational direction of the 
circular move according to the right-handed screw rule for a two dimensional 
circle and the shortest/longest path for a 3 dimensional circle. The termination 
type operand 508 for circular is defined to be the same as it is for linear. 

IV. B6zier Spline Interpolation 

[0165] Referring now to FIGS. 25-31, in another embodiment, rather than 
implement the coordinated move block 72 using coordinate transformations, the 
coordinated move block 72 is implemented using B6zier spline interpolation. 
According to this approach, the user is permitted to specify a plurality of 
weighting points in order to define a path profile. In response, the coordinated 
move block 72 generates a smooth path based on the specific boundary 
conditions which passes near the weighting points. 

[0166] Referring specifically to FIG. 25, FIG. 25 shows an example of a Bezier 
spline curve which may be generated by the coordinated move block 72. In 
FIG. 25, the user has specified five weighting points Wo-Wa. However, the user 
may also specify the starting and ending dynamics instead of the starting and 
ending weighting point Wo and MA. It will be understood that the number and 
placement of weighting points may vary from case to case as specified by the 
user. The weighting points may be specified by the user using the coordinated 
move instruction 200 or similar instruction modified to incorporate a plurality of 
fields configured to receive such weighting points. Based on the boundary 
control points Xo and X12 and the weighting points Wo-l/IA, the coordinated move 
block 72 identifies a plurality of control points (shown as points X^-X^^) and an 
associated plurality of polynomial path segments (shown as polynomials Po-Ps). 
Again, it will be understood that the number and arrangement of control points 
and polynomial segments varies based on the weighting points specified by the 
user. 
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[0167] In the example of FIG. 25, the endpoints of the path profile are defined 
by the control points Xo and X12. Thus, the move starts at the control point Xo 

with a starting velocity and a starting acceleration (determined by the 
previous move) and ends at the control point Xi2 with an ending velocity and 

an ending acceleration (determined by the next move). As will be detailed 

below, the remaining control points (Xi-Xn) are then calculated such that the 
first and second derivatives of the spline segments at the transition points (X3, 
Xe, Xs) between the path segments are equal. The path passes through the 
transition points but typically does not pass through the remaining control 
points. In the context of a path profile, where the curve represents position as a 
function of time, the control points are calculated such that the velocity vectors 
and the acceleration vectors are equal for adjacent path segments at the 
transition points. Also, within the constraint that the first and second 
derivatives must be equal for adjacent path segments, the remaining control 
points (X1-X11) are calculated such that the path segments (P0-P3) pass near the 
weighting points (i/Vb-l/lA). The control points may be calculated by a separate 
calculation instruction or during an initial phase of a coordinated move 
instruction. 

[0168] In the general case, the user may specify m-h 1 weighting points {Wo... 
Wm) resulting in m path segments {Po,..Pm^) Hence, in the example of FIG. 25, 
where five weighting points Wo-Wiare specified, there are four polynomial path 
segments P0-P3. For each path segment Pj (where y = 0, 1,2, 3, depending on 

the path segment), a plurality of coefficient vectors c^j , c, ^ , , and c^ j is 

calculated (not shown in FIG. 25). The control vectors are calculated based on a 
subset of the control points (Xsy, X3/+1, X3y+2, X3y+3). For example, for the path 
segment Pi, the control vectors are calculated based on the control points X3. 
X4, X5, and Xe, with the control points X3 and Xe being the transition points for 
the path segment Pi. The control point Xe is also a transition point for the path 
segment P2, and therefore, the coefficient vectors c^ j , c^ j , 03^, and c^j for 
the two path segments are calculated such that the starting velocity and 
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acceleration vectors for the path segment P2 are equal to the ending velocity and 
acceleration vectors for the path segment Pi at the transition point Xe. 

[0169] The coefficient vectors c^ j , c^ j , c^j , and c^ j are used during the 

dynamic phase of the profile generation (e.g., as the controlled element is 
moving) to calculate the current position, that is, to calculate the position 
reference values for the motion controllers 23. The B^zier spline polynomial is 
configured to provide the geometry of the path as a function of individual 
parameters uj which vary between 0 and 1 but which may in turn be specified as 
a function of time. Accordingly, the position reference values may be generated 
as a function of time and provided to the motion control loops to control 
operation of each of the motion control axes. Each calculated Pj value specifies 
the position of the curve in multiple dimensions (i.e., a multi-dimensional array), 
such that one coordinate may be provided to each of the plurality of motion 
control loops. 

[0170] As in the case of the coordinate transformation technique described 
above in Section II, B^zier spline interpolation may be used to generate 
transitioning path segments between a variety of types of path segments 
including linear, circular, and curved path segments. Also, rather than using 
B^zier spline interpolation only for transitioning path segments, the entire path 
could be constructed using B6zier spline interpolation. Because the path is 
calculated such that the velocity and acceleration are constant at path transition 
points and at the endpoints, BSzier spline interpolation provides a smooth path 
between points in space. Likewise, the calculations are sufficiently simple such 
that it is possible to change the profile dynamically during path execution. 

[0171] In Section IV(A), a mathematical characterization is provided of a single 
segment of a B6zier spline. Based on this knowledge and the requirement of 
smooth boundary conditions. Section IV(B) derives the general algorithm for 
Bezier spline interpolation. Section IV(C) provides the specific algorithms for 
Bezier splines transitions such as blending to and from linear, circular, or curved 
motion profiles. Section IV(D) describes the dynamics of a Bezier spline. 
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A. Mathematical Characterization of Single Bezier Spline Segment 

[0172] In this section, an equation for a single exemplary B6zier spline 
segment is provided. Initially, in this section, the exemplary Bezier may be any 
one of a starting path segment, an intermediate path segment, or an ending path 
segment in a Bezier path comprising multiple adjacent Bezier path segments. 
From the Bezier spline equation and known boundary conditions, equations for 
calculating the coefficient vectors c^j , c, ^ , c^j , and c^ j for the spline 
segment based on the control points (Xsy, Xay+i, X3/+2, X3/+3), and equations for 
calculating the first and second derivatives for the path starting points and 
ending points (P;(0) , P;(l) , P]{0) , and P;(l) ), are developed. 

[0173] The position Pi of a cubic Bezier spline segment; can be described by a 
cubic polynomial 

with the coefficient vectors {c^^ . ^c^^ } and the parameter w e [0,1]. 
However, this representation can be transformed into the standard Bezier 
representation with the introduction of the control points {Xsy, Xay+s} that 
satisfy the following boundary conditions: 

1 . The polynomial Pjfuj] starts at the control point Xsy: 

Pj(0) = X,j 

2. The polynomial Pjfuj) ends at the control point Xay+s: 

Pj(^) = ^3J.3 

3. The polynomial Pjfuj) starts with a first derivative defined by the control point 
X3/+1 and the multiplier jusj: 

P;iO) = JUsJ[^3J.^-^3J) 
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4. The polynomial Pj(uj) ends with a first derivative defined by the control point 
X3/+2 and the multiplier /jej\ 

5. The polynomial Pj(uj) starts with a second derivative that is independent of 
the control point Xay+a: 

6. The polynomial Pi(uj) ends with a second derivative that is independent of the 
control point Xay: 

P]{\) = g[x,j,,.X,j,,,X,j,,) 

Using the conditions 1 , 2, 3, and 4, the polynomial Pj(uj), and its first derivative, 

a set of equations for the coefficient vectors of the polynomial Pj(uj) can be 
derived as follows. 



^3, 

^37+3 

[0174] In order to solve the above set of equations, the coefficient vector c-^ j 
is separated from the second equation. 

Using the above result, the coefficient vector c^ j can be calculated. 
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= ^ + 2 * ^ + 3(x,j,, - c^j - c,j - c^j ) 

= -3*^-2*^-^ + 3*Jr3.>3 

= -3*c^j-2*c^j+3*X,j^,-Me,M^j*i-^ij*2) 

= -3*X,j-2* [X,j,, -Xy)+3* X,j,, - [X 3^,3 - X,j^^ ) 

Using the coefficient vector c^, the coefficient vector c^ j can be calculated. 

Cij ~ -X^j+i ~^0J ~^\J ~^2J 

= ^3y.3 - - (^3;.. - ^3, )- (2 * - 3K- + 2 * * X3,, - 

//.,y*^3y+2-(3-/^«jk3,>3 
= (2 - )X3,. -H * ^3y.. - /".J * ^37.2 + - 2)^3y.3 

[0175] Ail the coefficient vectors • -c^ } of the polynomial PyYtyy) are 

now represented based on the known control points {Xay, X3/+3} and the 
unknown multipliers ^Js.i and ^b>J. Using the conditions 5 and 6 and the second 
derivative of the polynomial Pi(ui) 

Pjiuj) = 2*cZj+6*'^j*Uj 
a set of equations for the multipliers /&,y and juej can be derived. 

f(^37»^3y+l'^3;+2) ~ 2 * y 

= 2(2 * Ms J - 3)^3,. - 4 * Msj * X^J.^ + 2 * Mej * ^ 3;.2 + 
2(3-y"e,>3,>3 

g(^3,>1.^37.2.^3y.3) = 2*^ + 6*^ 
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= 2(3 - Msj P^y + 2 * Msj * ^3.>. - 4 * MeJ * ^3v>2 + 

2(2 -3)^3 



■ 3y+3 



[0176] In order for the function f to be independent of the control point X3/+3 
and the function g to be independent of the control point Xa/, the multipliers /&.y 
and have to be equal to 3. With this result, the coefficient vectors can be 
simplified: 



C3 



- = 3(^3, -2*^3,,, +^3.>a) 

~ = - -^ij + 3(^3y+l - ^3y+2 )+ ^3;+3 



[0177] With the simplified coefficient vectors, the representation of the 
polynomial Pjfuj) can be transformed into the standard B6zier representation: 

= X,j + 3(- X,j + X,j^, + 3(^3, - 2 * X,J^^ + ^ 3.>2 Vj + 

= ^3,(l-3*«y +3*«j -«})+3*X3y..(".- -2*"j +"])+ 
^*X,j,,(u'j-u])+X,j^,*u] 

= X,j (1 - y + 3 * X,j^, (l-Uj}uj+3* X,j,, (1 - + ^3,>3 * " j 
[0178] With the introduction of the distances 
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the equations for the coefficient vectors and the first and second derivatives of 
the polynomial Pj(Uj) at the transition points can be further simplified: 



e ■ — c, ■ 



^3 J+3 ~ ^ij <^3j+2 



^3y+3 -^3J - 



p;(i) = 3(j^r3,,3-^3,,J 



pjiO) = f(^3,,^3,,„jr3,,J 

Pj(X) ~ 3 y+1 ' ^3 j+2 » ^3 j+3 ) 

" ^(d3J+3-<^3J+2) 
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B. Bezier Spline Interpolation 

[0179] Based on the knowledge of the equations for the coefficient vector 

Coy / ^ij i ^2] ' 3nd c^ j and the equations for the first and second derivatives at 

the path transition points (Pj(0) , Pj(l) , Pj(0) , and PJQ) ) as developed in the 

previous section for a single path segment, this section derives the general 
algorithm for Bezier spline interpolation over multiple path segments, including 
determining the locations of the control points Xi-Xsm i based on the boundary 
control points Xo and Xsm and the user-specified weighting points {Wo . . . Wm] 
(where m is the number of Bezier spline segments). Using this information, the 
equations for the coefficient vectors is also further simplified. 

[0180] As shown in FIG. 25, a cubic Bezier spline consists of a series of cubic 
B6zier spline segments with the Bezier spline passing through the control points 
{Xo, X3, X3m} and transitioning at the points {Xa, Xs/n-a} with a smooth 
first and second derivative. As a result, the polynomials Pjfu/) with 0 </ < m 
satisfy the following boundary conditions: 

1 . The polynomial Pjfu/) starts at the control point Xsj: 

Pj(0) = X,j 0<j<m 

2. The polynomial Pjfuj) ends at the control point Xay+s: 

P,(1) = X3.,3 0<j<m 

3. The first derivatives of the preceding polynomial Pj-^(uj-^) and the subsequent 
polynomial Pj(uj) are equal at the transition point Xay: 

P;_,(1) = P;(0) 0<j<m 

4. The second derivatives of the preceding polynomial h^(uj-\) and the 
subsequent polynomial Pj(uj) are equal at the transition point Xay: 

P]_,{\) = P]{0) 0<j<m 
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[0181] Using the second derivative of the polynomial Pj(uj) at the transition 
points and condition 4, a set of equations for the control points Xs/ 1 with 0 < j < 
m and X3/+1 with 0 </ < m can be derived. 



6(X3(^._i|,i " 2 * -^3(y-i)+2 + ^3(7-1)4-3 ) 



3y-2 



= 6(x3,-2*A^3,,,-hjr3,,J 



~ 2* A^jy^, - X^j^2 



With the specification of the weighting points 



~ 2 * ^ 3^^! - X 3 



~ 2^ X^j_^- X^j_2 

~ ^3 y-l (^3 7-1 ~ ^3 y-2 ) ^ ^3 y+1 ~ (^3y+2 ~ ^3y+l ) 



^3/-l +^3/-l 



^3 7+1 ^3 y+2 



the control points can be derived by the following set of equations. 

Wj_, +2*Wj = (2 * X,(j_,y, - X,^j^,^, )+ 2(2 * X,j_, - X,j_, ) 
= 2 * Jr3,_2 - X3,_, + 4 * ^3,_, - 2 * X3,.2 



= 3*X 



3y-i 



2 * + f^y., = 2(2 * X,j,, - X,j,, )+ (2 * X,y,,y, - X,y,,y, ) 



= 4 * X3,,, - 2 * X3,,2 + 2 * X,j,, - X,.,, 



= 3*X 



3y+i 



[0182] Using the above result, the first derivative of the polynomial Pjluh at 
the transition points, and condition 3, a set of equations for the control points 
Xsy with 0 < j < m can be derived. 
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) = 



p;(0) 



2*X 



3y 



37+1 



[0183] Therefore, the control points Xk with 0 < k < Zm (that is, the control 
points other than Xo and Xam) are defined by the following equations. 



^3/-l 



X 



3j+\ 



3 



0<j<m 
0<j<m 
0 < J <m 



The remaining control points Xo and Xsm are defined by the boundary conditions. 

[0184] With these results the equations for the coefficient vectors with 0 < J 
< m can be further simplified. 



:= Wj+2*Wj,,-2*Wj-Wj^, 
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= X 



37 



-3.7 



374-1 



^37+1 



= 3 



2I 3 3 . 

U2*Wj+Wj^,-Wj_,-2*Wj) 



W -W 

y+1 y-i 



^ 3(j3^.^2 -^37+1) 



J 1,7 



^37+3 "'^37 - 3*^3^^ 



^37+3 ~ ^37 ~ ^7 



3y+2 



C. B6zier Spline Transitions 

[0185] The remaining problem is to smoothly transition to and from a B6zier 
spline. As previously noted, Bezier spline interpolation may in one embodiment 
be used to connect linear, circular, and curved path segments. Accordingly, the 
transition to and from a B6zier spline can occur with or without a specific 
curvature. The first and last segments of the Bezier spline should have a 
curvature which matches the curvature (if any) of adjacent linear, circular, or 
curved path segments. In this section, simplified equations for the control points 
and the coefficient vectors are developed for the first and last path segments 
based on expected curvature of adjacent path segments. 
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1 , Transition without Curvature 

[0186] With reference to FIG. 27, the transition from a preceding motion 
profile to a Bezier spline occurs at the control point Xo with a given tangent , a 
constant first derivative, and a zero curvature. With the second derivative at the 
transition point 

and the requirement of a constant first derivative at the transition point 

^o"(0) = 0 

6(^-rf:) = 0 



the curvature is defined by 

= ||/>o'(0)xO|| 
= 0 

and the distance rf, and the control points Xi and X2 are defined by the 
following equations: 

^ = W,-X, 

3 
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[0187] With these results, the coefficient vectors can be simplified; 



-0,0 



-1,0 



-2,0 



-3,0 



= X, 
= 0 



[0188] The transition from a B6zier spline to a subsequent motion profile can 
be derived in a similar fashion. 



3m 



X 



3»i-2 



^3m "* ^3/M 



^ 3m-l " ^3m 



m-\ 



3m-l 



^3i«-3 ^m-\ 



3* /* , 
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" ^Bm ~ ^ 3m-3 ~ (-^3m ~ ) 
" fm-\ 

2. Transition with Curvature 

[0189] With reference to FIG. 28, the transition from a preceding motion 
profile to a B6zier spline occurs at the control point Xo with a given tangent 
and a given curvature ks. With the first and second derivative at the transition 
point 

= 3*^ 
OT) = 6(^-^) 
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and the definition of the curvature ks 



K, = ||p;(o)xp;(0)|| 



2\\d,x(d2-d,] 



3\\d, 



2 dixd-. 



3d 



sin(^«/^) 



2d, 



sin 



the length of the distance c/, is defined by the following equation. 



= \\W,-XJsm{<p) 



3*K, 



[0190] With reference to FIG. 29, in the case of a transition from a preceding 
circular motion profile to a B^zier spline with 
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r = ± 



and 



rsin((2>) = \W,-X, 



the equation for the length of the distance d^ can be further simplified. 



2 _ 



= \W,-X,\\sm{<p) 



3*K, 



= \W,-X,\\rsin{(p) 



[0191] Therefore, the distances and d^ and the control points X^ and Xz 
are defined by the following equations. 



= F.-^o||F 



V6 



^, = 

d~2 = 
X. = 



X^+d^ 



w,-x, 

2 

X.^dl 



[0192] With these results and 



-79- 



Atty. Dkt. No.: 02AB200 (081696-0251) 



3*d, 



the coefficient vectors are defined by: 



■'0,0 



= 3*J, 



-2,0 



-3,0 



^0 ^1,0 



^3 Xq — 



[0193] The transition from a B^zier spline to a subsequent motion profile can 
be derived in a similar fashion. 



*3/ii 



^3«-l 



*3/«-l 



||^3w ^m-\ II 



4e 



^3.-1-^.-1 



X 



3m-2 -^3m-l ~ ^3m-l 



m-l 



3(^3„-3-f^„-,) 



■'0,m-l 



3m-3 



3*rf 



3#n-2 



3(-^3m-2 " X 3„_3 ) 
= 3(fF„_,+^-^3„_3) 



= 3*t/3„..-3(^3„.3-fr„.,) 
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e. 



'm-\ 



C 



'm-\ 



c, 



e. 



'm-1 




C 




[0194] As previously noted, the B6zier spline interpolation described herein is 
preferably configured so as to be able to handle dynamic path changes, or 
changes to the path while a current move is being executed. In the case of a 
dynamic path change, the transition is referred to herein as a merge transition. 

[0195] In the case of a merge transition, the details of the preceding motion 
profile are unknown with the exception of the velocity and acceleration vectors 
at the transition point. These vectors define the starting velocity and starting 
acceleration at the control point Xo. For a transition with curvature, the 

starting velocity and acceleration are linearly independent and define the bi- 
normal vector 




the tangential, bi-normal, and normal unit vectors 



h 




n 
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and the curvature 



With the above results the merge transition can be derived from the circle 
transition by either defining the chord or arc length. 

[0196] According to the chord length method, with the chord length equal to 
the merge tolerance S 



w,-xA = 8 



and the results from the circle transition, the weighting point W^ can be 
calculated. 

^ = \w,-xl V 



V6 iv. 



= s - 



= K. 



3p, 
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[0197] With this result the control points X\ and Xi are defined, by the 
following equations. 

2 

[0198] According to the arc length method, with the arc length equal to the 
merge tolerance S 



r{2<p) = S 
2<p = S 



r 

and the results from the circle transition, the weighting point lVi can be 
calculated. 



= rsin(2^) 
—sm{6*K^) 
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«, = r(l - cos(2^)) 

— (l-cos(^*/cJ) 



[0199] With this result the control points Xi and X2 are defined by the 
following equations. 



= J . 

K 



= 2 ( ■ 

K 



— (sin ' (<5 * AT, ) + 1 - 2 cos(<5 * V J + cos ' * )) 

= 2 



= _4 



(l-cos(j**:J) 



■sm 



= 2 . (5*K^ 
— sin ^ 





V 


11^1 -^oll 




V6 





V6 



2 . r<j*/c;^ 

sm = 
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2 

[0200] With the results of either the chord or arc length method and 

el '■= 3*dl 

the coefficient vectors are defined by: 

^2,0 ^0 ~ ^1,0 

[0201] Advantageously, this blending technique is independent of the absolute 
value of the velocity and acceleration at the transition points, only involves the 
generation of one dynamic profile for the parameters uj, and does not require a 
dynamic coordinate transformation. However, the relationship between the 
parameter u/ and time t needs to be defined and the relationship between the 
parameter uj and arc length s is non-explicit. 

3. Blending Transition 

[0202] A blending transition is used to blend two motion profiles. Therefore, it 
is a combination of a transition from the preceding motion profile to a two- 
segment B§zier spline at the transition point Xo and from the Bezier spline to the 
subsequent motion profile at the transition point Xe. Since the two motion 
profiles are typically defined through an intersection point, which serves as the 
end point of the preceding profile and as the start point of the subsequent 
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profile, this intersection point is used as weighting point W^ for the Bezier spline. 
Based on the equations for transitions with and without curvature, the control 
points Xi, Xi, X4, and Xs can be calculated. The remaining control point is 
defined by the following equation. 

X-^ — X2 + X ^ 
2 

[0203] The above calculations can be simplified by applying a coordinate 
translation with the weighting point Wi as the new origin to the transition points 
Xo and Xe before calculating the remaining control points. 

[0204] With regard to a line-line transition, an example of a two-segment 
B6zier spline that blends from one linear motion profile to another linear motion 
profile with given transition point Xo and Xe and intersection point Wi is shown 
in FIG. 30. Based on the above results, the control points are given by 

^ = W,-X, 
3 

^2 = ^, +^ 

3 

>4 = x.-T, 

X-i = X2 + x^ 
2 
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[0205] With regard to a line-circle transition, an example for a two-segment 
B6zier spline that blends from a linear motion profile to a circular motion profile 
with given transition point Xo and Xe and intersection point W\ is shown in 
FIG. 31 . Based on the above results, the control points are given by 



3 

^2 = X.+J, 



V6 ||/, 



X, = X,-d, 



— + X ^ 



D. Bezier Spline Dynamics 

[0206] As indicated initially, the position Pj of a cubic Bezier spline segment j 
is described by a cubic polynomial 

[0207] with the coefficient vectors [c^ j . .c^ j } and the parameter uj e 
[0,1]. Above, the equations for the coefficient vectors {c^ j . .^3^ } have been 
described. However, it is noted that the position Pj of the cubic Bezier spline 
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segment is a function of uj , which is a parameter that is not inherently time- 
dependent but rather is defined simply as varying between zero and one (uj e 
[0,1]). Accordingly, the following two subsections describe two methods for 
generating uj as a function of time. This, in turn, allows Pj to be calculated as a 
function of time. 

1 . Arc Length Based Dynamics 

[0208] The arc length based dynamics method uses the knowledge of the arc 
length sAuj) as a function of the parameter. With this function the overall arc 
length k of a Bezier segment and the overall arc length / of a Bezier spline are 
defined. 

m-1 

[0209] During the initial phase of the profile instruction, the arc length / is 
used as distance to go and the starting and ending velocity and acceleration vs, 
Ve, as, and ae are used as boundary conditions in order to generate a motion 
profile x(f) as a function of time. During the dynamic phase of the profile 
generation, this motion profile is used to calculate a specific distance x(f/) at a 
given time. The specific distance x[ti\ is then transformed back to a specific arc 
length sy(x(f/)), which in turn is transformed back into a specific parameter 
uAsAx^m. 

[0210] The arc length based dynamics method is advantageous in that it is a 
simple way of generating constant velocity profiles. However, this approach 
involves solving the complex relationship between arc length and the parameter 
Uj and the fact that velocity and acceleration may exceed maximum values at 
sharp corners. 

2. Parameter Based Dynamics 
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[0211] The parameter based dynamics method uses the knowledge of the 
starting and ending velocity and acceleration v^^. , v^j , a^ j , and a^j for each 
BSzier segment j. These values and the following equations 



= dP. du, 
dt 



d'p, 



dUj 



= dP. du, 
dUj dt 

= P'ji\)*Uj{t^) 



du] 



r("y(^))* 



du 



dt 



dP, d^u, 



pj(0)*u'jit,)+p;(_o)*uj(tj 



a 



e.J 



PjiUjitJ) 

d^P,. fdu,. Y dP, d'u. 

dUj dt J dUj ' dt 



= p;(i)*u'jit,)+p;(i)*ujitj 
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are used to calculate the starting and ending first and second derivatives of the 
parameter for each B6zier segment /. 



U • 



[0212] During the initial phase of the profile instruction, the parameter range 1 
is used as distance to go and the starting and ending velocity and acceleration 
"5,7 ' ^ej ' ^sj ' ^ej 3re used as boundary conditions in order to generate a 
motion profile uAt) as a function of time. During the dynamic phase of the profile 
generation, this motion profile is used to calculate a specific parameter uAti) at a 
given time. 

[0213] The parameter based dynamics method of specifying uj as a function of 
time is advantageous in that there is no need to solve the complex relationship 
between the arc length and the parameter uj and the velocity and acceleration is 
limited to the maximum values at sharp corners. However, generation of 
constant velocity profiles is more difficult. 

[0214] The techniques described herein extend to those embodiments which 
fall within the scope of the appended claims, regardless of whether they provide 
any of the above mentioned advantageous features. Many changes and 



s,j\ 



'3j+l 



^37+3 



^ej-4d3j.3-dy,2)^lj\\ 



3F37-3 
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modifications may also be made to the invention without departing from the 
spirit thereof. The scope of these changes will become apparent from the 
appended claims. 
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